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Abstract 


This  line  of  research  seeks  to  increase  knowledge  of  a  tracked  target  using  the  particle 
filter,  also  known  as  Sequential  Monte  Carlo  (SMC)  methods.  The  target  is  tracked  using 
vision  based  observations.  These  observations  were  simulated  using  both  dual  cameras 
and  a  single  camera.  If  only  a  single  camera  tracks  the  target,  depth  cannot  be  determined 
directly  and  is  considered  an  unobservable  state.  Filters  can  estimate  this  unobservable 
state  using  a  dynamic  model  and  data  from  the  image.  However  the  movement  of  the  target 
is  nonlinear  which  eliminated  filters  traditionally  used  to  track  motion  such  as  the  Kalman 
filter  and  its  variants.  The  particle  filter  is  an  alternative  that  can  track  nonlinear  motion, 
but  was  not  feasible  until  recently  due  to  its  computational  requirements.  Simulations  of 
nonlinear  target  movement,  first  in  two  dimensions,  then  three,  evaluated  the  particle  filter’s 
feasibility  and  performance.  Subsequent  simulations  evaluated  the  particle  filter’s  ability 
to  track  a  target  using  dual  and  single  camera  observations.  Evaluation  tests  were  devised 
to  characterize  the  performance  of  each  filter.  Analysis  metrics  were  produced  to  analyze 
the  results  of  these  tests.  Einear  and  Kalman  filters  were  also  devised  to  serve  as  additional 
comparisons  to  the  particle  filter.  Results  for  dual  camera  observations  demonstrated  the 
filter  could  track  the  target  and  determine  unobservable  states,  however  results  for  the  single 
camera  observations  indicated  the  filter  was  problematic  since  it  could  not  return  accurate 
depth  estimates  and  suffered  from  severe  weight  collapse. 
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COMPUTER  VISION  TRACKING  USING  PARTICLE  EILTERS  EOR  3D  POSITION 


ESTIMATION 


I.  Introduction 


1.1  General  Issue 

With  the  advent  of  computers  approximately  50  years  ago,  there  is  a  general  trend 
pushing  computer  systems  to  simulate  natural  abilities,  such  as  vision  and  motion.  What 
seems  natural  to  humans,  such  as  spotting  and  following  a  moving  object  such  as  a  Erisbee, 
is  challenging  for  computers.  The  two  elements  used  by  humans  are  the  eyes  to  see  and 
follow  the  target,  and  the  brain  to  process  the  images.  The  analogous  computer  components 
are  pan/tilt/zoom  Pan-Tilt-Zoom  (PTZ)  cameras  to  ’see’  and  track  the  target  and  software 
to  process  the  images  from  the  cameras.  Humans  are  innately  able  to  differentiate  between 
a  moving  target,  other  targets,  the  background,  and  any  appearance  of  movement  due  to 
movement  of  the  eyes  or  heads.  Computers  are  unable  to  perform  these  differences  without 
complex  software.  Several  key  challenges  confront  these  artificial  systems,  among  them, 
the  ability  of  a  system  to  identify  moving  targets  and  attempt  to  follow  them  by  predicting 
their  path  against  a  moving  background  (i.e.  simulating  moving  eyes  and  heads  of  humans). 
The  focus  of  this  thesis  explores  a  potential  method  of  predicting  the  movement  of  targets 
by  using  the  particle  filter.  The  particle  filter  attempts  to  determine  the  location  and  predict 
the  velocity  and  heading  of  a  target  as  much  as  humans  do  when  catching  a  Erisbee. 

1.2  Problem  Statement 

The  research  in  this  thesis  attempts  to  answer  two  problems  that  currently  face  visual 
tracking  systems,  in  particular  the  visual  tracking  system  being  developed  at  the  Air  Eorce 


1 


Institute  of  Technology  (AFIT).  The  first  problem  is  the  ability  of  the  PTZ  cameras  to  track 
a  target.  Presently,  the  tracking  algorithm  looks  for  a  change  in  a  target’s  position  and  does 
not  attempt  to  account  for  an  target’s  changing  velocity  when  moving  the  PTZ  cameras  to 
follow  the  target.  The  tracking  algorithm  does  not  account  for  a  target’s  changing  velocity 
because  the  only  incoming  data  is  how  the  target  moved  relative  to  its  last  position,  or 
optical  flow.  The  particle  filter  attempts  to  predict  the  target’s  3-D  position,  velocity  and 
heading  estimates  so  the  cameras  can  more  accurately  follow  the  target.  The  particle  filter 
uses  a  non-linear  model  of  a  target’s  movement  in  3-D  space.  The  second  problem  is  depth 
determination  using  a  single  camera.  Typically  two  or  more  cameras  are  used  to  determine 
the  depth  or  how  far  away  a  target  is.  A  single  camera  is  unable  to  make  this  determination 
since  the  camera  does  not  know  if  the  target  is  changing  size  or  moving  towards  or  away 
from  the  camera.  The  only  data  available  is  the  target’s  2-D  optical  flow;  side-to-side  and 
up-and-down  movement,  along  with  the  velocities.  The  particle  filter  attempts  to  predict 
the  depth  based  on  the  2-D  optical  flow  and  the  velocities  as  well  as  any  change  in  the 
target’s  scale  size. 

1.3  Research  Objectives/Questions/Hypothesis 

This  research  focuses  on  characterizing  the  ability  of  the  particle  filter  to  determine 
the  states  of  a  target  in  3-D  space  using  two  cameras  or  using  a  single  camera.  The  sub¬ 
objectives  that  must  be  accomplished  to  fulfill  this  goal  are  detailed  below. 

•  Develop  a  general  model  of  the  tracked  object’s  dynamics 

•  Determine  if  the  particle  filter  can  determine  hidden  states 

•  Develop  a  particle  filter  using  measurements  from  two  cameras  and  evaluate  its 
performance  using  simulated  data 

•  Develop  a  particle  filter  using  measurements  from  a  single  camera  and  evaluate  its 
performance  using  simulated  data 
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1.4  Research  Focus 


The  focus  of  this  research  is  to  evaluate  the  viability  of  the  particle  filter  to  a),  predict 
a  target’s  position,  velocity,  and  heading  states  and  b).  predict  depth  of  a  target  using  a 
single  camera.  The  filter  results  will  be  compared  with  a  known  truth  to  provide  a  baseline 
comparison. 

1.5  Methodology 

Model  development  and  simulated  data  collection  will  occur  within  the  MATLAB 
environment.  The  path  and  states  of  the  target  moving  through  Three-Dimensional  (3-D) 
space  will  be  generated  using  a  non-linear  model.  A  collection  of  points  for  the  simulated 
camera  to  track  will  be  generated  around  the  moving  target  point.  The  particle  filter  will  use 
the  simulated  camera’s  measurements  of  these  points  as  the  measurement  data  and  generate 
its  predicted  location  of  the  target  in  3-D  space.  Both  the  dual  camera  and  single  camera 
particle  filter’s  will  follow  this  procedure. 

1.6  Assumptions/Limitations 

The  movement  of  a  target  in  3-D  space  is  typically  nonlinear,  thus  several  assumptions 
concerning  the  model  must  be  made  in  order  to  make  the  problem  tractable  theoretically. 
The  model  assumes  no  changes  in  acceleration  rates  of  any  states.  The  target  is  assumed  to 
not  change  in  size  (i.e.  the  model  does  not  change  in  form  in  any  way).  Thus  any  change  in 
size  detected  by  the  camera  correlates  exclusively  to  a  change  in  distance  from  the  camera. 
These  model  assumptions  limit  the  effectiveness  of  the  filter,  however  a  simpler  model 
provides  a  basis  for  expansion  into  more  complex  models.  Noise  inherent  in  the  model  is 
assumed  to  be  Gaussian  white  noise,  that  is  noise  with  a  normal  distribution  and  zero  mean. 
Additionally,  the  state  and  measurement  variances  must  be  known  or  approximated  so  that 
the  filter  can  be  tuned  properly. 
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1.7  Implications 

The  PTZ  system  the  particle  filter  could  be  implemented  on  uses  two  cameras  to 
determine  the  location  of  the  target  and  tracks  based  on  position  of  the  target.  The  particle 
filter  has  the  potential  to  enable  accurate  velocity  tracking  using  only  position  data.  With 
accurate  velocity  estimates,  better  predictions  of  the  target’s  future  movement  could  be 
made.  With  better  predictions,  the  PTZ  cameras  could  be  steered  to  more  accurately  to 
track  the  target  resulting  in  better  tracking.  Additionally,  the  particle  filter  also  has  the 
potential  to  reduce  the  number  of  cameras  to  a  single  camera  decreasing  system  complexity 
and  cost. 

1.8  Overview 

The  subsequent  chapter  is  a  review  of  the  literature  concerning  the  optical  flow 
algorithm,  and  the  motivation  to  select  the  particle  filter.  Additionally,  Chapter  2  also 
details  the  methodology  of  the  particle  filter  and  how  it  works  to  return  state  estimates. 
Chapter  3  describes  several  simple  particle  filters,  referred  to  as  prototype  particle  filters 
Prototype  Particle  Filter  A  (PPF-A)  and  PPF-B.  These  prototype  filters  were  developed 
to  both  demonstrate  the  particle  filter  as  well  as  verify  basic  properties  of  the  particle 
filter,  notably  its  ability  to  determine  hidden  states.  Chapter  4  describes  the  target  model 
and  particle  filters  used  to  simulate  the  PTZ  system  for  both  dual  and  single  cameras. 
Note,  these  particle  filters.  Evaluation  Particle  Filter  A  (EPF-A)  and  Evaluation  Particle 
Eilter  B  (EPE-B),  are  different  from  the  prototype  filters  PPE-A  and  PPE-B.  Chapter  5 
discusses  the  data  analysis  of  EPE-A  and  EPE-B  and  extrapolates  results  from  the  data 
analysis.  Chapter  6  elaborates  on  the  conclusions  drawn  from  the  data  analysis  and  provides 
recommendations  for  future  research  and  development. 
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II.  Literature  Review 


2.1  Overview 

With  the  increase  in  computational  speed  and  portability  as  well  as  high-definition 
digital  optics,  there  has  been  a  significant  amount  of  research  undertaken  into  the 
development  of  vision-based  real-time  tracking  systems  [1].  The  current  evaluation  system 
used  for  this  report  has  well  developed  tracking  capabilities,  using  optical  flow  background 
estimation  as  its  basis  [10].  However  it  presently  possesses  a  linear  Kalman  filter  for  target 
tracking,  which  is  not  optimized  for  non-linear  target  tracking.  Chapter  2  summarizes 
the  applicable  research  conducted  concerning  optical  flow  processing  as  it  relates  to  the 
tracking  system  used  along  with  a  discussion  on  filters,  with  a  focus  on  particle  filters. 

2.2  Photogrammetry 

Photogramme  try  is  the  process  of  determining  3-D  coordinates  through  images.  The 
mathematical  underpinnings  of  photogrammetry  are  rooted  in  the  1480s  with  Leonardo 
da  Vinci’s  study  of  perspectives  [8,  p.  1].  However,  digital  photogrammetry  did  not 
emerge  until  the  development  of  the  digital  cameras  in  the  1970s.  A  close  relative  of 
photogrammetry,  videogrammetry,  determines  information  based  on  multiple  images  taken 
over  time.  Currently  photogrammetry  and  videogrammetry  are  used  in  a  variety  of  fields 
from  topographic  mapping  to  film  motion  capture  and  special  effects  [18,  p.  4]. 

2.3  Optical  Flow 

Photogrammetry,  as  it  pertains  to  this  research,  aims  to  determine  the  three- 
dimensional  coordinates  of  the  target’s  centroid  based  on  multiple  two-dimensional  feature 
points  seen  by  the  camera.  Features  represent  the  object  in  an  image,  and  a  feature  is 
required  to  determine  the  location  of  a  point  or  set  of  points  that  represent  an  object;  hence 
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the  term  feature  points.  Feature  points  are  points  that  can  be  well  described  mathematically. 
Typical  features  and  definitions  are  as  follows:  [12,  p.  2-5] 

•  Color  -  the  apparent  color  of  an  object  is  influenced  by  two  physical  factors:  1) 
spectral  power  distribution  of  the  illuminant;  2)  the  surface  reflectance  properties  of 
the  object 

•  Edges  -  strong  changes  in  image  intensities  defining  object  boundaries;  less  sensitive 
to  illumination  changes 

•  Optical  Flow  -  dense  field  of  displacement  vectors  defining  the  translation  of 
brightness  for  each  location  in  consecutive  frames 

•  Texture  -  statistical  measures  of  the  intensity  variation  of  a  surface  which  quantifies 
the  properties  such  as  smoothness  and  regularity;  less  sensitive  to  illumination 
changes 

While  color  is  a  popular  feature,  the  tracking  algorithm  developed  by  Doyle  uses  optical 
flow  to  detect  feature  points  [12].  The  motivation  behind  this  choice  is  that  optical  flow  is 
able  to  track  a  wider  range  of  targets  than  other  methods  that  are  more  sensitive  to  a  target’s 
physical  properties.  Moving  a  camera  results  in  a  shift  of  the  background  image.  All 
tracking  algorithms  must  discern  between  actual  target  movement  and  apparent  movement 
induced  by  the  movement  of  the  cameras.  By  using  optical  flow  to  track  the  target’s 
movement,  any  additional  movement  of  the  background  can  also  be  tracked  using  optical 
flow  and  then  subtracted  from  the  target’s  apparent  movement  to  reveal  the  target’s  actual 
movement.  Another  method  to  improve  tracking  is  to  train  a  system  to  recognize  the  target; 
however  training  is  undesired  in  order  to  allow  the  tracking  algorithm  to  function  on  a 
variety  of  targets  and  surfaces.  Thus,  none  of  the  filters  used  target  training. 
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2.3.1  Limitations  of  Existing  System. 

As  typical  with  many  photogrammetry  systems,  X  and  Y  coordinates  are  relatively 
accurate,  however  Z  (depth)  coordinates  are  slightly  less  accurate  [12].  Additionally,  the 
current  system  requires  two  cameras  for  depth  estimation  and  can  only  track  one  object 
at  a  time.  Although  the  current  system  tracks  position  and  velocity  states  using  a  Kalman 
filter,  the  weight  for  velocity  is  minimal  due  to  the  filter’s  uncertainty.  This  uncertainty 
stems  from  the  simple  control  law  used  by  the  filter,  to  regulate  the  target  to  the  center  of 
the  camera’s  vision  field. 


2.4  Filters 

In  order  to  produce  meaningful  information  concerning  the  location  of  the  tracked 
object  in  the  global  frame,  the  incoming  pixel  data  must  be  filtered.  Although  a  variety  of 
methods  exist,  this  research  focused  on  Bayesian  based  filters  for  reasons  detailed  in  the 
subsequent  sections.  The  filter  ultimately  used,  the  particle  filter,  is  explored  in  depth  in 
Section  2.5. 

2.4.1  Bayes’  Theorem. 

Bayes’  Theorem  is  named  for  Thomas  Bayes  and  was  heavily  edited  and  updated  by 
Richard  Price  and  published  in  1763  [3].  The  theorem  evaluates  the  probability  that  a 
proposed  hypothesis,  or  prediction,  is  correct  given  given  observational,  or  measurement 
data,  about  that  hypothesis. 


Pimm  = 


p(e)xP(D|e) 

P(D) 


(2.1) 


P  (H)  is  the  prior  or  initial  degree  of  belief  in  the  hypothesis,  or  predicted  states. 
represents  the  probability,  or  how  strongly  the  data  D,  or  measurements,  support  the 
hypothesis.  The  result  is  posterior  probability,  or  degree  of  belief  in  the  hypothesis 
having  accounted  for  the  data  D.  Using  multiple  hypotheses  with  the  same  data  produces 
a  Probability  Density  Function  (PDF).  The  PDF  embodies  all  available  statistical 
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information  and  may  be  described  as  the  complete  solution  to  an  estimation  problem 
[2,  p.  174].  Specifically,  the  PDF  describes  the  relative  likelihood,  probability,  that  a 
particular  variable  will  be  equal  to  a  certain  value.  However,  the  Bayesian  estimation  is 
sensitive  to  noise  that  affects  the  measure  of  the  hypothesis  or  states  x  [6].  Thus  due  to 
the  amount  of  noise  present  in  the  data  or  measurements  of  the  pixels  as  well  as  in  the 
camera  PTZ  movements,  a  filter  is  needed  that  can  recover  the  actual  state  values  from 
noisy  measurements  y.  The  subsequent  filters  utilize  the  state-space  model  below: 

Xf+i  =  ^(X(,Uf,W() 

yt  =  h  ix„  Vf)  (2.2) 

The  functions  g  and  h  describe  state  and  measurement  propagation,  x  is  the  state,  u  is 
the  process  input,  y  is  the  measurement,  w  and  v  are  the  system  (or  state  or  process)  and 
measurement  noise  vectors,  and  t  is  the  discrete  time  [20]. 

2.4.2  Bayes  Filter. 

An  extension  of  the  Bayes’  Theorem  is  Recursive  Bayesian  Estimation  also  known  as 
the  Bayes  Filter  or  Grid-Based  Filter.  The  variables,  Xo:f  and  yj.^  are  denoted  in  Equation  2.3 
and  2.5  [11,  p.  5]. 


X():,  =  [Xo,  ...,X,} 

(2.3) 

yi-.t  =  {yi— .yJ 

(2.4) 

These  are  the  state  values  and  measurements  up  to  time-step  t,  respectively.  Using  a 
given  system  and  measurement  model,  the  Bayes  Filter  calculates  a  new  posterior  PDF, 
p(xo:flyi:(),  as  well  as  the  marginal  posterior  PDF,  p(xjyi.j),  at  each  time-step  based  on 
the  previous  state  estimate  and  new  incoming  measurement.  The  marginal  posterior  is  also 
known  as  the  filtering  distribution  or  posterior  filtered  density.  The  filtering  distribution 
returns  the  probabilities  of  various  values  of  variables,  the  states  in  this  research,  without 
reference  to  the  values  of  other  variables.  This  is  the  posterior  PDF  of  the  states  x  at  a 
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single  time  step,  t.  Ultimately,  the  filtering  distribution  is  the  PDF  of  interest  sinee  the  state 
probabilities  are  separate  and  not  joint.  The  posterior  PDF  is  given  by  Bayes’  Theorem  at 
any  time  t,  as  seen  in  Equation  2.5  [2,  p.  108]. 


p{xt\y,_i)  =  J  p(xj|Xf_i)p(x,_i|y,_i)  JXf_i 


(2.5) 


A  reeursive  formula  for  the  posterior  PDF,  p  (xo:,|yi.f),  is  derived  and  shown  in  Equation  2.6 
[11,  p.  6]. 


P(X0:«+llyi  :t+l  )  =  p(xojlyi.J 


p  (y,^jjx,+i)  p  (xi+ijx,) 


(2.6) 


p(y(+ilyi:^) 

The  filtering  distribution  ean  be  ealeulated  in  a  two  step  reeursion  proeess:  the  predietion 
and  update,  as  seen  in  Equation  2.7  and  2.8  [11,  p.  6]. 


Predietion:  p(X(|yi.,_i)  =  J p(x,jx,_i) pix^.ily^.^dx^. 

p(ytMp(xtjyi.j_i) 


Update:  p  (x^ly^^,)  = 


(2.7) 

(2.8) 


A  prior  PDE,  or  the  predietion  as  seen  in  Equation  2.7,  for  the  state  x^.  is  obtained  using 
the  Chapman- Kolmogorov  equation  based  on  the  measurements  up  to  t  -  1  [21,  p.  531]. 
The  state  is  first  predieted  with  the  state  propagation  belief  p(x,|Xf_i)  using  the  model 
/.  Then  this  predietion  is  eorreeted  by  the  measurement  likelihood  p  (Xf_ilyj.j_i).  The 
update  follows  Bayes’  Theorem  to  ealeulate  the  posterior  PDE  for  state  x^,  aeeounting  for 
measurements  up  to  time  step  t,  yj.,.  These  expressions  however,  are  deeeptively  simple. 
In  reality,  one  eannot  typieally  ealeulate  the  normalizing  eonstant,  p(y,),  and  the  marginals 
of  the  posterior,  p(Xf|y,),  sinee  they  require  the  evaluation  of  eomplex  high-dimension 
integrals  [11,  p.  6].  Eurthermore,  the  Bayes  Eilter  seales  poorly  in  reality  due  to  the 
large  state  spaee  needed  for  multidimensional  state  veetors.  Multidimensional  integrals  are 
needed  to  ealeulate  the  prior  probability  of  eaeh  point  in  the  state  spaee.  These  integrals  also 
grow  and  beeome  inealeulable  as  the  state  spaee  grows.  Additionally,  the  state  spaee  must 
be  diseretized  or  eertain  limitations  must  be  plaeed  on  the  model  for  eomputer  eomputation. 
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Much  research  has  been  devoted  to  developing  approximations  for  these  distributions.  The 
subsequent  seetions  diseuss  some  of  the  eommon  filters  used.  Eaeh  of  the  following  filters 
attempts  to  approximate  the  posterior  PDF 

2.4.3  Kalman  Filter. 

The  Kalman  filter  is  a  popular  type  of  Bayes  filter  with  several  restrietions  to  solve  the 
problem  of  state  estimation  eneountered  in  the  Bayes  Filter  [11]  [20]: 

•  g  and  h  must  be  linear  funetions 

•  w  and  V  must  be  uneorrelated,  additive  Gaussian  white  noise,  with  zero  mean  and 
known  varianee 

The  Kalman  filter  is  unsuitable  to  model  a  target  moving  in  three  dimensional  spaee  with 
ehanging  veloeity  and  aeeeleration.  A  model  with  ehanging  veloeity  and  aeeeleration  is 
highly  likely  to  be  non-linear  and  the  Kalman  filter  requires  linear  models.  However,  the 
Kalman  filter  serves  as  a  useful  eomparison  to  the  partiele  filter  and  a  seeond  order  Kalman 
filter  is  used  as  a  eomparison  within  Chapters  4  and  5.  The  Kalman  filter  is  broken  into  two 
steps,  the  predietion  and  update,  like  the  Bayes  filter  it  emulates. 

2.4.3. 1  Kalman  Filter  Prediction  Step. 

The  predietion  step  eonsists  of  a  predietion  of  the  states,  %,  and  a  predietion  of  the 
error  eovarianee  matrix,  Pf,  whieh  is  an  estimate  of  how  aeeurate  the  state  estimate  is. 
Additional  parameters  are  the  state  transition  model  matrix.  A,  the  eontrol-input  model,  B, 
the  eontrol  veetor,  Ujt,  and  the  system  noise  eovarianee  matrix,  Q.  Equation  2.9  and  2.10 
deseribe  the  predietion  step  [4,  p.  411-470]  [7,  p.  231-279]. 

X,  =  AX(_i  -I-  But  (2.9) 

P,  =  AP,_iA^  +  Q  (2.10) 
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2.43.2  Kalman  Filter  Update  Step. 

The  update  step  occurs  once  measurements  are  received  by  the  filter.  The  update 
step  calculates  the  Kalman  gain,  Kf,  which  is  a  measure  of  the  relative  certainty  of  the 
state  estimate  and  measurements  at  each  time  step.  The  predicted  states  updated  with  the 
Kalman  gain  the  measurements.  The  final  update  is  an  update  of  the  error  covariance  matrix 
using  the  Kalman  gain.  The  measurements  used  are  defined  by  the  measurement  model,  C; 
some  corresponding  states  may  be  observable  and  some  may  be  hidden  (no  measurements 
exist  for  them).  Equation  2.11,  2.12,  and  2.13  describe  the  update  process  [4,  p.  411-470] 
[7,p.  231-279]. 


K,  =  (CP,C^  +  R)“' 

(2.11) 

Xj  =  X  -t  Kf  (y,  -  CX() 

(2.12) 

P,  =  (I-K,C)P, 

(2.13) 

Two  key  weaknesses  of  the  Kalman  filter  that  are  made  clear  are  the  inability  to  change  the 
system  and  measurement  noise  covariance  matrices,  Q  and  R,  and  the  inability  to  change 
its  dynamics  (A.  For  a  second-order  Kalman  filter,  the  control  law  defines  the  acceleration 
values;  if  the  target  acceleration  is  not  constant,  the  Kalman  filter  will  begin  to  lag.  A  vision 
tracking  system  possesses  a  significant  problem  since  the  PTZ  cameras  may  no  longer  move 
fast  enough  to  capture  the  target  if  the  filter  is  used  to  control  the  cameras. 

2.4.4  Extended  Kalman  Filter. 

To  combat  the  limitations  of  the  Kalman  filter,  namely  its  requirement  for  linear 
functions,  the  Extended  Kalman  Filter  (EKE)  was  developed  at  NASA  Ames  [19]  [22].  The 
EKE  approaches  the  linear  problem  by  linearizing  a  process  before  estimating  it  similarly 
to  a  Kalman  filter.  The  EKE  calculates  the  Jacobian  of  /  and  h  around  the  estimated  state, 
thus  producing  a  trajectory  of  the  model  function  centered  around  the  state  in  question 
[20].  Unfortunately  it  can  still  fail  to  accurately  represent  the  model  unless  a  number  of 
specific  parameters  are  applied  rending  the  filter  unsuitable  for  situations  requiring  long 
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sequences  or  sudden  changes  in  the  state  of  the  target  [11,  p.  5]  [6,  p.  3].  Furthermore, 
if  the  initial  estimate  is  incorrect,  the  filter  may  quickly  diverge  due  to  its  linearization 
around  the  estimate.  Such  a  condition  is  unsuitable  for  tracking  since  initial  estimates  of 
the  target’s  position  and  velocities  are  likely  to  be  inaccurate.  The  original  purpose  of  the 
EKF  was  to  predict  navigation  properties  for  orbital  flybys.  The  creators  assumed  that  the 
spacecraft  would  not  deviate  far  or  quickly  from  its  intended  path  and  thus  the  process 
could  be  linearized  around  the  estimate  [19,  p.  614-615]. 

2.5  Particle  Filter 

The  origins  of  the  particle  filter,  also  known  as  Sequential  Monte  Carlo  (SMC) 
methods,  actually  predate  the  Kalman  filter  by  several  years.  The  fundamental  basis  of 
the  particle  filter  was  established  by  Hammersley  in  1954,  wherefore  he  argued  that  SMC 
methods  could  be  used  to  estimate  the  posterior  PDF  of  the  state-space  by  using  Bayesian 
recursion  equations  [14].  However,  it  was  not  until  1993  that  Gordon  et  al.  demonstrated 
that  SMC  methods  are  suitable  for  tracking  or  other  applications  that  previously  relied  on 
Kalman  filters  or  their  variants  [13].  Particle  filters  were  also  not  practical  until  sufficient 
computer  power  was  available  to  run  the  filter  in  real  time.  The  significant  benefit  of  the 
particle  filter  is  that  no  restrictions  are  placed  on  /,  or  ht,  or  on  the  distributions  of  the  system 
or  measurement  noise  [13,  p.  108-109].  Furthermore,  the  particle  filter  is  not  dependent 
on  knowledge  of  prior  states,  only  the  present  state.  This  aspect  is  discussed  further  in 
Section  2.5.1.  The  prime  difference  between  the  particle  filter  and  Bayes’  or  Grid  Filters 
is  that  although  both  seek  to  determine  the  PDF,  the  particle  filter  does  so  discretely  using 
a  set  of  random  samples,  or  particles,  instead  of  as  a  function  over  the  state  space.  As  the 
number  of  particles  increases,  they  provide  a  more  accurate  representation  of  the  PDF. 

2.5.1  Goal  of  the  Particle  Filter. 

The  goal  of  a  particle  filter  is  to  estimate  the  posterior  PDF  of  the  state  variables 
given  the  measurements.  However,  as  discussed  in  Section  2.4.2,  determining  the  actual 
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continuous  PDF  is  typically  impossible,  and  if  not  impossible,  eomputationally  expensive, 
slow,  and  prone  to  noise.  Rather  than  determine  the  posterior  PDF,  the  particle  filter 
estimates  the  PDF  discretely  by  generating  a  Diserete  Probability  Distribution  (DPD). 
There  are  two  elements  to  the  DPD:  a  probability  distribution  and  a  Probability  Mass 
Funetion  (PMF).  The  probability  distribution  is  a  sample,  at  time-step  t  of  n  =  ...,N 

samples  drawn  from  the  proposal  distribution,  N  being  the  total  number  of  samples  drawn. 
These  samples  are  also  known  as  the  partieles.  The  proposal  distribution  is  a  DPD 
that  estimates  the  desired  posterior  PDF  and  may  also  be  referred  to  as  the  importance 
sampling  distribution,  the  importanee  funetion,  or  the  importanee  density,  depending  on 
the  literature  souree,  and  is  diseussed  further  in  Seetion  2.5.3.  The  PMF  is  referred  to  as 
the  importanee  weights.  Conceptually,  the  probability  distribution  is  the  spread  of  partieles 
within  the  state-spaee  before  the  weights  are  assigned.  The  importanee  weights  assign 
the  probabilities  to  this  distribution.  The  weights  represent  the  likelihood  of  a  particle 
eontaining  the  truth  states.  Figure  2.1  illustrates  the  two  elements  of  the  DPD.  Several 


State  Space:  S  Particles 


Figure  2.1:  Visual  Depiction  of  the  Diserete  Probability  Distribution 


assumptions  regarding  the  model  must  be  made  before  using  the  particle  filter  [1 1,  p.  5]. 
•  The  state  process,  is  a  first  order  Markov  process  that  ean  be  modeled  as: 


X,|X,_1  ~  /7x,|x,_i(x|x,_i) 
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with  an  initial  DPD  of  p{xq).  A  Markov  process  is  a  process  that  satisfies  the  Markov 
property.  A  process  satisfies  the  Markov  property  if  predictions  for  future  states  can 
be  made  based  only  on  present  states  just  as  well  as  if  all  previous  states  were  known. 

•  Measurements  are  conditionally  independent  provided  that  x,  is  known;  that  is, 
each  measurement  is  only  dependent  on  its  corresponding  state  x, 

yjx,  ~  Px|y(y|xr) 

A  visual  illustration  of  the  state-space  is  seen  in  Figure  2.2. 


Figure  2.2:  The  State-Space 


Section  2.5.2  through  2.5.4  detail  the  development  of  the  elements  used  by  the  particle 
filter.  The  actual  particle  filter  process  is  detailed  in  Section  2.5.5. 

2.5.2  Monte  Carlo  Methods. 

As  mentioned  in  Section  2.5,  the  particle  filter  is  based  on  Monte  Carlo  (MC)  methods. 
MC  methods  seek  to  address  the  problems  encountered  with  Bayes’s  Filter,  detailed  in 
Section  2.4.2.  MC  methods  approximate  the  integrals  present  in  Equation  2.6,  2.7,  and 
2.8  [11,  p.  6].  However,  MC  methods  usually  cannot  sample  from  the  the  posterior  PDF 
p(xo:flyi:,)  at  any  time  t  [11,  p.  8].  Thus,  alternate  methods  were  developed  to  address  this 
issue. 
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2.5.3  Importance  Sampling. 

Importance  Sampling  (IS)  is  a  technique  to  estimate  the  properties  of  a  specific 
PDF  using  samples  generated  from  a  different  PDF,  assuming  these  PDFs  are  directly 
proportional  to  each  other.  Suppose  p{x)  oc  n{x),  where  p{x)  is  a  PDF  from  which  it 
is  difficult  or  impossible  to  draw  samples  from,  but  n{x)  may  be  easily  sampled  from. 
Additionally,  p{x)  may  only  be  evaluated  up  to  proportionality.  Let  these  samples  be 
defined  as  .r"  sampled  from  p(x),n  =  1,...,A  and  generated  from  n(x),  the  proposal 
distribution.  These  samples  also  have  weights  associated  with  them,  the  importance 
weights,  that  allow  p(x)  to  be  characterized.  Equation  2.14  and  2.15  define  the  weighted 
approximation  of  p(x)  and  the  importance  weights,  where  6  is  the  Dirac  delta  function  [2, 
p.  178]. 


N 

p(x)  =  ^  a)"6  (x  -  x") 

n=l 

p(x) 


CO  oc 


7r(x) 


(2.14) 

(2.15) 


Applying  this  to  the  research,  the  desired  posterior  DPD  is  defined  by  Equation  2. 16,  while 
the  importance  weights  for  the  samples,  Xq.^,  are  defined  by  Equation  2.17[2,  p.  178]. 


N 


P  (X0:Jyi:,)  =  OoT^5  (xo:f  -  xJJ) 


rt=l 

p(4i\yv.t) 


7T 


(2.16) 

(2.17) 


However,  a  key  concern  with  IS  is  that  it  requires  all  measurements  before  estimating 
P  (xo:flyi:()-  Thus,  for  each  new  measurement  that  becomes  available,  the  importance 
weights  must  be  recalculated  over  the  entire  state  sequence.  This  aspect  renders  IS 
inadequate  for  recursive  estimation.  Naturally,  the  computational  requirements  and 
complexity  increase  with  time,  either  resulting  in  a  lagging  filter  or  inaccurate  results  [11, 
p.  9]. 
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2.5.4  Sequential  Importance  Sampling. 

Sequential  Importance  Sampling  (SIS)  solves  the  concerns  of  IS  by  modifying  IS 
so  that  an  estimate  of  j!7(xo:(|yi.,)  may  be  determined  without  modifying  the  previous 
state  sequence  =  I,...,A/^|.  The  proposal  distribution  is  factorized  according  to 

Equation  2.18[11,  p.  9]. 


^(xo:rlyi:f)  =  ;r(xo:,_i|yi:,_i);r(x,|xo:f_i,yi:,) 


(2.18) 


Iterating  Equation  2.18  results  in  the  expression  seen  in  Equation  2.19[11,  p.9]. 


^(xo:;lyi:f)  =  7r(Xo)]^;r(x,|Xo:(-l,yi:f) 


(2.19) 


This  importance  function  allows  the  weights  to  now  be  evaluated  recursively  in  time,  as 


demonstrated  in  Equation  2.20[11,  p.  9]  [2,  p.  178]. 

qxi"VS_..y.J 


(2.20) 


This  process  of  SIS  is  simplified  further  by  using  the  transition  prior  DPD,  p  (xq:,),  as  the 
proposal  distribution,  shown  in  Equation  2.21[11,  p.9]. 


^(X0:(lyi:f)  OC  ;?(Xo:f)  =  f  {Xq) 


(2.21) 


By  using  the  transition  prior  DPD,  only  x\  must  be  stored,  both  the  path  x[,.^_j  and  history 
of  measurements,  yi-^.i  may  be  discarded.  When  the  filtering  distribution,  p{xt\yi.i)  is 
the  posterior  PDE  of  interest,  the  proposal  distribution  only  depends  on  x^-i  and  y,.  The 
filtering  distribution  and  importance  weights  may  be  approximated  using  Equation  2.22[2, 
p.  178]  and  2.23[ll,p.  10]. 


P  (X(lyi:,)  «  ^  oJl6  (x,  -  x”) 

n=\ 

iof  oc  w^'^\p  (yjx|"^) 


(2.22) 


(2.23) 
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One  problem  encountered  by  SIS  is  that  as  t  increases,  the  distribution  of  importance 
weights  becomes  skewed  as  preference  is  given  to  higher  weights.  Eventually,  one  one 
particle  has  a  non-zero  importance  weight.  Thus,  the  SIS  fails  to  accurately  represent  the 
posterior  PDFs  of  interest,  namely  the  filtering  distribution.  An  additional  re-sampling  step, 
known  as  bootstrapping,  is  used  to  help  mitigate  this  collapse[ll,  p.lO]. 

2.5.5  Particle  Filter  Process. 

The  particle  filter  evaluated  for  this  research  is  known  as  a  Bootstrap  Particle  Filter 
(BPF)  [2,  p.  178].  The  process  it  uses  is  based  on  SIS,  however  it  adds  an  additional  step 
to  address  the  skewed  weights  present  in  the  SIS.  The  basic  concept  of  the  BPF  is  to 
eliminate  particles  with  low  importance  weights  and  increase  the  number  of  particles  with 
high  importance  weights  [13].  The  BPF  can  be  broken  into  three  basic  steps,  each  with 
several  sub-steps.  The  three  steps  are:  initialization,  importance  sampling,  and  selection 
[11,  p.  11].  Figure  2.3  provides  a  visual  depiction  of  the  BPF  process  in  Section  2. 5. 5. 5. 
The  steps  within  Figure  2.3  are  referenced  within  the  three  steps. 

2.5.5. 1  Initialization. 

Before  the  BPF  can  begin  filtering  measurements,  an  initial  DPD  must  be  generated. 
This  initial  DPD  is  based  on  the  initial  conditions  of  the  state-space  model  in  Equation  2.2. 
The  particles  are  distributed  based  on  state  noise,  w  or  system  noise  variance,  cr^  (x).  The 
initialization  occurs  at  timestep  t  =  0. 

•  For  n  =  1,  sample  p(xo)  and  set  t  =  1  [11,  p.  11] 

2.5.5. 2  Importance  Sampling. 

The  importance  sampling  is  where  the  proposal  distribution  is  sampled  and  is  Step  1 
on  Figure  2.3.  As  mentioned  in  Section  2.5.4,  the  proposal  distribution  is  the  prior  DPD, 
which  is  the  probability  distribution  that  expressed  uncertainty  about  the  states  before 
measurements  are  taken  into  account.  It  attributes  a  degree  of  uncertainty,  rather  than 
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randomness,  to  the  uncertain  states.  This  uncertainty  is  expressed  via  the  system  noise 
variance,  x  represent  the  unweighted  predicted  states  or  proposal  distribution. 

•  For  n  =  \, ...,  N,  sample  from  p  (xf|x|”^J  and  set  Xq”)  =  x[”^j  [1 1,  P-  1 1] 

The  importance  sampling  step  also  uses  the  system  noise  variance  to  add  variety  to  the 
proposal  distribution,  reflected  in  Step  4  on  Figure  2.3.  Variety  must  be  added  after  the  re¬ 
sampling  step  since  re-sampling  creates  multiple  samples  with  the  same  states,  in  essence, 
reducing  the  number  of  unique  states.  This  aspect  is  further  discussed  in  Section  2.5. 5. 3 
and  2. 5. 5.4.  Step  1  on  Figure  2.3  assumes  that  variety  was  introduced  previously.  The 
importance  weights  are  then  sampled.  These  importance  weights  are  approximations  of 
the  relative  posterior  probabilities  of  the  particles.  In  other-words,  each  weight  represents 
the  likelihood  that  its  corresponding  particle  (containing  the  estimated  states)  is  correct 
compared  to  the  true  states. 

•  For  n  =  1, ...,  V,  evaluate  the  importance  weights  [11,  p.  11] 


CO 


(n) 

t 


(2.24) 


The  importance  weights  are  then  normalized,  forn  =  l,...,V[ll,p.  11]. 


',(«)  _ 


CO 


(n) 


yN  Ai 
2jn=l 


(n) 


(2.25) 


2.5.5.3  Selection. 

The  BPF  resamples  the  particles  based  on  their  weights  using  a  weighted  roulette 
selection  [11,  p.  10]  and  is  Step  3  on  Figure  2.3.  Thus,  particles  with  greater  weights  are 
apt  to  be  sampled  more  frequently.  Different  re-sampling  algorithms  have  been  developed, 
though  this  research  relies  on  the  algorithm  developed  by  Gordon  et  ah,  which  is  one  of 
the  most  popular,  and  is  described  in  Section  2. 5. 5.4  [13]  [11,  p.  10].  Regardless  of  the 
resampling  algorithm  chosen,  all  follow  a  similar  method. 
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•  Resample  with  replacement  particles  z  =  1,  from  the  set  (xq”];zz  =  l,...,A/^j 
according  to  the  importance  weights  [1 1,  p.  11] 

•  Set  t  =  t  +  \  and  proceed  to  the  importance  sampling  step 

As  for  the  actual  estimated  states,  x,,  these  can  be  determined  by  a  variety  of  statistical 
methods  that  analyze  the  posterior  DPD,  p  (Xilyj.,).  For  this  research,  a  mean  was  taken  of 
the  estimated  states. 

2. 5. 5. 4  Resampling  Algorithm. 

After  the  importance  weights  are  normalized,  a  random  number,  r„,  is  sampled 
from  a  standard  uniform  distribution  between  0  and  1,  which  matches  the  range  of 
normalized  importance  weights.  The  normalized  weights  are  summed  cumulatively  until 
their  cumulative  sum  is  greater  than  r„.  The  index,  z,  of  the  last  normalized  importance 
weight  summed  is  used  to  retrieve  the  predicted  state  vector  corresponding  to  that  weight. 
This  state  vector  becomes  the  first  discrete  point  in  the  posterior  DPD.  This  process  is 
repeated,  using  a  new  each  time,  for  the  total  number  of  particles,  N,  at  which  point  the 
posterior  DPD  is  created.  Equation  2.26  describes  these  steps,  where  t  is  the  time-step  and 
n  is  the  particle  number. 

=  Yjm=l  Wn 

i  =  argmax;^!  ^zZz[z]  :  dz[z]  >  (2.26) 

As  a  consequence  of  the  re-sampling  step,  multiple  particle  samples  are  assigned  to  the 
same  state  vector,  since  the  objective  of  the  re-sampling  step  is  to  eliminate  particles 
with  low  importance  weights  and  multiply  particles  with  high  importance  weights  [11, 
p.  10].  Without  introducing  variety,  the  particle  filter  would  begin  to  suffer  from  weight 
degeneration  and  eventually  succumb  to  weight  collapse.  Weight  degeneration  and  weight 
collapse  are  discussed  further  in  Section  2.5.6.  To  help  prevent  weight  degeneration,  the 
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sampling  step  assigns  variety  to  the  DPD  ereated  by  the  re-sampling  step,  seen  in  Step  4  on 
Figure  2.3. 

2.5. 5. 5  Visual  Depiction  of  Particle  Filter  Process. 

Figure  2.3  provides  a  visual  illustration  of  the  BPF  [11,  12]. 


In  Figure  2.3,  represents  the  population  of  samples,  and  N~^ ,  the  weights  or 
PMF.  The  BPF  starts  at  a  time-step  t  with  an  unweighted  measure  or  importanee  sampling 
distribution  |xj"\  whieh  is  an  approximation  of  p  (x,|yi.y_i).  The  importanee  sampling 
distribution  already  ineorporates  variety.  Importanee  weights  are  ealeulated  using  the 
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measurements  at  time  t,  which  is  an  approximation  of  p(X(|yj.,),  the  posterior  PDF  of 
interest  from  which  statistical  metrics  are  computed  to  generate  an  estimate  of  the  states. 
The  re-sampling  step  then  selects  on  the  fittest  or  most  heavily  weighted  particles  to  create 
the  importance  sampling  distribution  Note,  this  is  still  an  approximation  of 

p{xt\yi.t),  the  particles  are  simply  reassigned  to  reflect  the  current  weights.  Variety  is 
introduced  with  the  state  update  at  time-step  t  +  \.  Customarily,  and  for  this  research, 
the  variety  is  provided  using  the  system  noise  variances.  These  variances  are  tailored  to 
the  particle  filters  used  in  this  research  and  discussed  in  Chapter  4  and  5.  This  variety 
results  in  the  new  importance  sampling  distribution,  |x["\  which  is  an  approximation 
ofp(Xf|yi:,+i). 

2.5.6  Limitations  of  the  Particle  Filter. 

Beyond  the  increased  computational  needs,  particle  filters  do  have  several  limitations. 
One  of  the  most  common  limitations  is  weight  disparity  which  may  lead  to  weight  collapse 
[11,  p.lO].  Weight  degeneration,  occurs  when  all  but  a  few  of  the  importance  weights 
are  close  to  zero.  When  most  of  the  importance  weights  are  close  to  zero,  the  particle 
filter  cannot  produce  a  good  posterior  density  since  only  a  few  wights  will  be  sampled 
repeatedly  subsequently  skewing  the  mean.  This  degeneration  continues  to  worsen  as  the 
mean  becomes  more  inaccurate  with  each  time  step  until  weight  collapse  occurs  once  all  but 
one  of  the  importance  weights  are  close  to  zero.  Most  computer  algorithms  crash  once  the 
sole  particle  not  close  to  zero  approaches  zero  likehood  (due  to  predicted  measurements 
drifting  farther  from  actual  measurements)  and  the  algorithm  attempts  to  normalize  the 
weight(s)  by  dividing  by  zero.  Various  methods  can  mitigate  weight  collapse,  the  most 
common  being  a  re-sampling  step  whenever  the  number  of  effective  particles  drops  below 
a  certain  threshold.  The  number  of  effective  particles  can  not  be  directly  determined,  but 
may  be  estimated  using  Equation  2.27  [2,  p.l79]. 


1 


eff 


(n\2 


(2.27) 
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where  wf^  is  the  normalized  weight.  Neff  is  less  than  N  and  a  small  Neff  indicates 
severe  degradation.  More  sophisticated  particle  filters,  such  as  the  Sequential  Importance 
Resampling  (SIR)  filter,  Auxiliary  Sampling  Importance  Re-sampling  (ASIR)  filter,  and 
Regularized  Particle  Filter  (RPF),  also  seek  to  eliminate  weight  collapse  through  a  variety 
of  methods,  though  for  the  purposes  of  this  research,  these  filters  were  not  investigated  nor 
implemented  [2,  p.  180-183].  Particle  filters  also  depend  upon  the  model  of  the  system 
in  question.  Although  the  particle  filter  can  compensate  for  noisy  measurements  and  non¬ 
linear  systems,  if  the  model  is  sufficiently  poor,  the  filter  is  unable  to  perform  adequately. 

2.5.7  Uses  of  the  Particle  Filter. 

Due  in  part  to  its  newness,  the  particle  filter  has  not  been  widely  implemented  and  is 
still  in  a  development  stage.  Thus,  most  of  the  users  of  the  particle  filter  are  not  commercial 
applications,  but  experimental  research.  The  particle  filter  and  its  variants  have  been  used 
for  facial  recognition  and  tracking  under  varying  light  and  background  situations  [16]. 
Additional  research  at  the  University  of  Florence  resulted  in  a  first  order  tracking  particle 
filter  for  targets  moving  in  two  dimensions  [6].  Research  sponsored  by  the  U.S.  Army 
Research  Laboratory  and  U.K.  Ministry  of  Defense  also  explored  using  Bayesian  filtering 
to  track  multiple  targets  via  proximity  sensors  [15].  Although  much  of  the  research  utilizing 
the  particle  filter  is  concerned  with  target  tracking,  the  particle  filter  may  be  applied  to  a 
variety  of  situations  with  unknown  states.  One  such  area  is  determining  the  remaining 
useful  life  prediction  of  lithium-ion  batteries  [17]. 

2.6  Conclusion 

Although  the  particle  filter  has  seen  limited  application  for  tracking,  discussed  in 
Section  2.5.7,  no  research  indicated  any  attempt  to  use  the  particle  filter  to  track  a  target 
within  3-D  space  based  on  the  position  and  movement  of  the  target’s  centroid  using  only 
position  measurements.  Additionally,  no  research  indicated  any  attempt  to  use  the  particle 
filter  to  estimate  depth  without  actual  depth  measurements. 
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III.  Methodology:  Prototype  Filters 


3.1  Introduction 

This  chapter  details  the  prototype  particle  filters  developed  to  test  and  evaluate  the 
potential  to  use  particle  filters  to  determine  the  hidden  states  of  a  moving  target  using 
camera  measurements. 

3.2  System  Design  Approach 

Although  the  concept  of  the  particle  filter  predates  more  mature  filters  such  as  the 
Kalman  filter,  development  of  the  particle  filter  lags  behind  other  filters.  Thus,  the  first  stage 
is  to  develop  a  prototype  particle  filter  within  the  MATLAB  environment  and  determine 
if  hidden  states  can  successfully  be  determined  based  on  limited  measurements.  Once 
verified,  particle  filters  for  both  a  dual  camera  system  and  single  camera  system  will  be 
developed.  Included  in  this  stage  is  the  development  of  a  target  model  to  both  generate 
simulated  measurements  for  the  particle  filter  and  to  be  used  within  the  particle  filter  to 
generate  particle  distributions.  The  target  model  will  differ  slightly  for  each  camera  setup, 
dual  and  single  respectively.  Two  prototype  particle  filters,  A  and  B,  were  developed  to 
evaluate  various  aspects  of  the  particle  filter. 

3.3  Prototype  Particle  Filter  A:  Single  Variable  Tracking 

The  first  filter  developed,  PPF-A,  was  based  on  the  filter  created  by  Godon  et  al. 
[13].  The  primary  purpose  of  PPF-A  was  to  serve  as  a  software  validation  of  the  particle 
filter  within  the  MATLAB  environment.  PPF-A  will  also  serve  as  the  framework  on 
which  all  subsequent  particle  filters  will  be  developed.  PPF-A  uses  the  same  state,  Xt,  and 
measurement,  y,,  models  as  the  filter  created  by  Gordon  et  al.  The  same  noise  covariances 
in  the  state,  Wk,  and  measurement,  Vt  were  also  used.  Equation  3.1  and  3.2  describe  the 
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model  used. 


Xt  =  O.Sxf-i  +  +  8eos(1.2(f  -  1))  +  Wt  (3.1) 

J,  =  |  +  v,  (3.2) 

The  variables  Wt  and  v,  are  zero-mean  Gaussian  white  noise  with  state  and  measurement 
varianees  of  10  and  1  respeetively.  Additionally,  the  number  of  partieles,  N,  used  was  500 
and  the  initial  state  was  a:o  =  0.1.  An  example  run  is  seen  in  Figure  3.1. 


Figure  3.1:  Prototype  Partiele  Filter  A:  Non-linear  Funetion  Demonstration 


Figure  3.1  demonstrates  the  partiele  filter’s  ability  to  traek  a  non-linear  target  funetion 
given  noisy  measurements.  The  ability  to  traek  non-linear  target  is  a  key  differenee  between 
the  partiele  filter  and  other  filters  sueh  as  the  Kalman;  the  partiele  filter  aeeepts  higher 
order  estimates  of  perfeet  order.  Perfeet  order  estimates  are  estimates  that  are  not  truneated 
or  linearized  as  they  must  be  in  the  Kalman  filter.  Due  to  the  highly  non-linear  nature 
of  the  ehosen  target  funetion,  seen  in  Equation  3.1,  a  Kalman  filter  was  not  used  as  a 
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comparison,  however  future  functions  within  Chapter  3,  4,  and  5  will  use  the  Kalman  filter 
as  a  comparison  to  the  particle  filter. 

3.4  Prototype  Particle  Filter  B:  Multi- Variable  Tracking  and  Hidden  State  Determi¬ 
nation 

PPF-B  evaluated  the  particle  filter’s  tracking  ability  with  the  presence  of  hidden  states; 
that  is,  states  that  can  only  be  predicted  within  the  model  since  they  do  not  correspond 
directly  to  a  measurement. 

3.4.1  Target  Model. 

The  target  model  used  contained  both  x  and  y  positions  as  well  as  a  velocity  magnitude 
V  and  a  velocity  heading  angle  6.  Both  positions  vary  with  velocity,  velocity  changes  with 
a  constant  acceleration,  a,  and  the  heading  6  is  held  constant.  Additionally,  system  noise  is 
introduced  into  the  model  for  both  a  and  9  via  cr^ixa)  and  the  system  noise  variances 

for  acceleration  and  heading  velocity  respectively.  When  system  noise  is  introduced,  a  and 
6  are  no  longer  constant.  The  variance  for  each  state,  cr^(x),  provides  a  measure  of  the 
spread  of  that  state’s  density  function.  Often,  and  for  this  research,  the  standard  deviation, 
a{x)  is  used  in-place  of  the  variance  to  denote  the  spread  of  the  density  function.  The 
addition  of  system  noise  simulates  the  presence  of  modeled,  higher-order  functions,  such 
as  changes  in  acceleration.  System  noise  is  not  added  to  the  target  positions  u  and  v  since 
the  target  positions  are  derived  from  V  and  9  and  ultimately  the  acceleration  values,  a  and 
9.  Vn  is  a  normally  distributed  random  number  that  is  used  to  generate  the  system  noise.  It 


25 


changes  each  time  it  appears.  Equation  3.3  through  3.8  describe  the  target  model. 


Mr.r  =  htj-i  +  ^1,1-1  •  sin(07’^f_i)  •  At  (3.3) 

VT,t  =  vjj-i  +  Vj^t-i  •  cos(07’,/_i)  •  At  (3.4) 

=  yT,t-i  +  ciT,t-i  •  At  (3.5) 

0T,t  =  +  dT,t-\  •  At  (3.6) 

aT,t  =  aT,t-i  +  ^Jcr\xT,a)  ■  rn  (3.7) 

Stj  =  ^r,/-i  +  yj o'^(xT,e) '  (3.8) 

3.4.2  Measurements. 


The  measurements  passed  to  the  filter  are  the  positions  u  and  v.  Measurement  noise 
is  added  to  simulate  noisy  measurements  the  filter  would  receive  in  reality.  This  noise 
is  represented  by  the  measurement  noise  variances,  cr^{mT,u)  and  cr^{mT,v),  for  uj  and  vj 
respectively.  The  equations  representing  these  measurements  are: 


^fn,t  —  ^T,t  ' 

yJcr^(mT,u)  ■  rn 

(3.9) 

^m,t  =  ^T,t  +  - 

sJo-H.mT,v)  ■  rn 

(3.10) 

3.4.3  Particle  Filter  Model. 

The  PPF-B  model  used  mimics  the  target  model,  however  it  does  not  contain  a  specific 
acceleration  variable  since  acceleration  is  unknown  and  the  motivation  is  to  determine 
velocity  and  heading  states.  Rather,  any  changes  in  acceleration  are  treated  as  system 
noise.  Like  the  target  model,  the  filter  contains  its  own  system  noise  variances,  (T^(xfy) 
and  cr^ixfg),  for  V  and  0,  to  simulate  those  present  in  the  model.  Again,  these  system  noise 
variances  represent  the  target  acceleration  values.  Ideally  these  will  match  the  actual  noise 
variances  of  the  target  model.  These  variances  are  also  what  the  filter  uses  to  generate  its 
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distribution  of  particles. 


Ufj  =  Uf^t-i  +  ■  sin(0/,f_i)  •  At  (3.11) 

Vfj  =  Vfj-i  +  Vf^t-i  ■  cos(0/,(_i)  •  At  (3.12) 

Vf,  =  +  ^(rHxfy)  ■  r„  (3.13) 

Of^t  =  Of,t-\  +  yjcr^Xffi)  ■  Tn  (3.14) 


The  PPF-B  measurements,  Ufm  and  Vfm,  are  set  equal  to  the  target  measurements  and  v^. 
These  measurements,  are  compared  to  the  target  measurements,  y^.  Importance  weights, 
oj,  are  generated  from  this  eomparison  for  each  particle,  n,  as  detailed  in  Equation  3.15. 
These  weights  are  based  on  the  probability  of  the  particle  measurement  being  correct 
given  the  actual  measurement  while  accounting  for  measurement  noise,  represented  by  the 
variances  cr^(m/_„)  and  cr^(myv).  Ideally,  these  filter  measurement  noise  varianees,  cr^(mf), 
will  match  the  target  measurement  noise  variances,  cr^iniT).  The  smaller  the  measurement 
covariances  are,  the  more  the  filter  trusts  the  measurements  and  vice-versa.  A  normal  or 
Gaussian  distribution  was  used  to  generate  the  weights,  however  any  type  of  distribution 
may  be  used  to  model  the  target  noise,  so  long  as  the  same  distribution  is  used  within 
the  filter.  Both  measurements,  Um  and  Vm  were  weighted  equally,  as  defined  by  the  weight 
matrix,  in  Equation  3.16  [13].  By  setting  the  weights  equal,  this  means  the  filter  assumes 
both  the  measurements,  eorresponding  to  each  weight,  are  of  equal  importanee.  Weights 
are  typically  set  at  at  lower  values  if  their  corresponding  measurements  were  less  aceurate 
and  the  user  does  not  want  these  measurements  to  adversely  affeet  the  state  estimation. 
Weights  are  typically  set  at  higher  values  if  their  corresponding  measurements  are  of  greater 
interest  and  the  user  desires  the  filter  pay  more  attention  to  these  measurements  and  their 
corresponding  states  at  the  exelusion  of  others. 


-  yS)  •  %  •  (yf,t  - 

2cr(my) 


{n) 

co:  ^  = 


^2ncr(mf) 


■EXP 


(3.15) 
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Cl(u)  0 

Qf  =  (3.16) 

[  0  n(v) 

3.4.4  Kalman  Filter  Model. 

Two  Kalman  filters  serve  as  comparisons.  Prototype  Kalman  Filter  A  (PKF-A)  uses 
the  same  initial  conditions  as  the  target  and  the  same  acceleration  values  as  the  target,  which 
are  reflected  in  the  control  vector  u.  Prototype  Kalman  Filter  B  (PKF-B)  uses  the  same 
initial  conditions  as  the  PPF-B  and  slightly  different  acceleration  values,  to  illustrate  the 
lag  that  occurs  when  the  control  vector  is  incorrect.  Since  the  Kalman  filter  must  use  linear 
functions,  the  non-linear  functions  of  velocity  and  heading  are  derived  from  the  component 
velocities  of  u  and  v,  ii  and  v  respectively,  as  seen  in  Equation  3.17  and  3.18. 

Ut  =  V-cos(et)  (3.17) 

Vt  =  V-sm(et)  (3.18) 


Equation  3.17  and  3.18  are  also  applied  to  the  control  law  and  covariance  matrices  when 
applicable.  In  order  to  provide  a  comparison  to  the  target  and  PPE-B,  ii  and  v  values  are 
transformed  to  their  respective  V  and  6  values  using  Equation  3.19  and  3.20. 


(3.19) 

(3.20) 


Equation  3.21  through  3.26  describe  the  the  state  transition  model  matrix.  A,  the 
control-input  model,  B,  the  control  vector,  xx^,  the  system  noise  covariance  matrix. 


Q,  the  measurement  model,  C,  and  the  measurement  noise  covariance  matrix,  R. 


1  0  At  0 
0  1  0  At 

A  =  (3.21) 

0  0  10 

0  0  0  1 


—  0 
2  ^ 

0  ^  \u 

B  =  ^  (3.22)  \Xk  =  (3.23) 

At  0  V 

0  At 
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o-(Xu)  0 


0 


0 


0  cr  (Xv)  0  0 

Q  =  (3.24) 

0  0  cr  (xy)  0 

0  0  0  cr  (xe) 


1  0  0 

0 

o-{yv) 

0 

c  = 

(3.25) 

R  = 

0  1  0 

0 

0 

o-iye) 

(3.26) 


3.4.5  Simulation  Scenarios. 

Three  simulations  were  conducted,  changing  various  parameters  of  the  model  and 
filter  to  validate  the  claim  that  particle  filter  could  determine  hidden  states.  Initial 
conditions  for  the  target,  PKF-A,  PKF-B,  and  PPF-B  are  provided  in  Table  3.1. 


Table  3.1:  PPF-B  Scenario:  Initial  Conditions 


States 

u 

V 

V 

e 

a 

e 

Target 

10 

10 

1 

30 

1 

2 

PKF-A 

10 

10 

1 

30 

1 

2 

PKF-B 

0 

0 

0 

0 

0 

0 

PPF-B 

0 

0 

0 

0 

N/A 

N/A 

Additional  parameters  used  throughout  the  three  scenarios  are  provided  in  Table  3.2. 
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Table  3.2:  PPF-B  Scenario:  Parameters 


Filter 

PKF-A 

PKF-B 

PPF-B 

Number  of  Simulations 

100 

100 

100 

Run  Time 

100 

100 

100 

Time  Step 

1 

1 

1 

Number  of  Particles 

N/A 

N/A 

500 

3. 4.5.1  Scenario  1:  No  System  or  Measurement  Noise. 

The  first  scenario  contained  no  system  nor  measurement  noises  in  the  target  model. 
Thus,  the  measurements  observed  were  completely  accurate.  Target  system  variances, 
o'^(xt,v)  and  cr^(xTfi)  were  both  0  as  measurement  noise  variances,  cr^(mf  u)  and  cr^{mf^y) 
respectively.  Kalman  filter  system  covariance  values  were  set  to  0. 1  and  not  0  for  both 
V  and  6  in  order  to  to  propagate  the  error  covariance  prediction  and  the  Kalman  gain 
calculation.  The  measurement  covariance  values  were  set  to  0  for  both  measurements  u  and 
V.  Particle  filter  system  noise  covariances  were  not  0  to  match  the  target,  since  the  filter  still 
must  create  a  distribution  of  particles,  but  they  were  kept  low  to  reflect  the  lack  of  system 
noise  with  cr^(v/,y)  and  cr^ixffi)  both  equal  to  1.  The  measurement  noise  variance  used  by 
the  filter,  cr^(mf),  was  equal  to  0.1  reflecting  both  the  lack  of  target  measurement  noise,  but 
also  the  uncertainty  introduced  into  the  filter  from  the  filter  system  noise  covariances.  The 
mean  of  the  difference  between  each  filter  value  and  the  target  value  for  all  the  simulation 
runs  at  each  time  step  are  calculated  and  the  shown  in  Figure  3.2.  Rather  than  separate 
u  and  V  position  variables,  the  Euclidean  distance  is  used  instead  on  the  basis  that  both 
positions  should  be  accurate.  Additionally,  the  results  from  a  single  simulation  are  provided 
in  Figure  3.3. 
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Distance  Error 


Theta  Heading  Error 


Figure  3.2:  PPF-B:  Mean  Error  for  No  System  or  Measurement  Noise 
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Heading  (Deg)  Velocity 


u  Position 


Velocity  Magnitude 


Heading 


Figure  3.3:  PPF-B:  Single  Run  for  No  System  or  Measurement  Noise 
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As  seen  in  Figure  3.2  and  3.3,  the  filter  is  able  to  determine  the  velocity  and  heading 
once  the  filter  locks  onto  the  u  and  v  position.  Initially  there  is  an  overshoot  and  undershoot 
of  the  velocity  and  heading  to  reflect  the  increase  in  velocity  needed  to  match  the  simulated 
measurements  to  actual  measurements  since  the  filter  lags  behind  the  target  due  to  differing 
initial  conditions.  Scenario  1  does  not  evaluate  the  performance  of  the  filter,  but  functions 
as  a  diagnostic  on  the  filter’s  functionality.  As  discussed,  the  filter  system  and  measurement 
noise  covariances  cannot  be  set  to  zero,  nor  would  they  ever  since  a  system  with  zero 
noise  does  not  need  a  filter  who’s  purpose  is  to  remove  noise.  Unsurprisingly,  given  the 
lack  of  noise  and  relative  linearity  of  the  system,  both  Kalman  filters  performed  better 
than  PPF-B  for  position  and  velocity  estimation.  However,  neither  Kalman  filter  returned 
accurate  heading  estimates.  This  is  likely  due  to  the  errors  in  the  velocity  estimation  that 
are  compounded  when  predicted  in  heading,  as  seen  in  Equation  3.20.  Also  as  expected, 
velocity  errors  for  PKF-B  were  greater  than  PKF-A  due  to  the  erroneous  control  law. 
Table  3.3  provides  the  mean  error  values  for  each  filter  for  each  variable  over  the  last 
50  time  steps.  The  last  50  were  chosen  in  order  to  allow  PKF-B  and  PPF-B  to  track  to  the 
target  since  neither  began  with  the  target’s  initial  conditions. 


Table  3.3:  Mean  Errors  for  East  50  Time  Steps  without  System  or  Measurement  Noise 


Metrics 

u 

V 

V 

e 

PKE-A 

0 

0 

1.4337 

49.4796 

PKE-B 

0 

0 

1.8665 

49.796 

PPE-B 

1.6738 

3.1969 

3.1448 

4.3499 
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3.4.5.2  Scenario  2:  Measurement  Noise,  No  System  Noise. 

.  Scenario  2  contained  measurement  noise  but  no  system  noise.  The  presence  of 
measurement  noise  increases  the  uncertainty  imparted  on  the  filter  from  the  measurements. 
The  variances  for  system  noise  are  provided  in  Table  3.4. 


Table  3.4:  PPF-B  Scenario  2:  Variances 


Variance 

O-ijuf 

criyvf 

PKF-A 

10 

10 

PKF-B 

10 

10 

PPF-B 

10 

10 

As  with  Scenario  1,  the  mean  errors  were  calculated  and  can  be  seen  in  Figure  3.4. 
The  results  from  a  single  simulation  run  are  provided  in  Figure  3.5. 
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Figure  3.4:  PPF-B:  Mean  Error  for  No  System  Noise 
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Figure  3.5:  PPF-B:  Single  Run  for  No  System  Noise 
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As  seen  in  Figure  3.5,  despite  the  presence  of  the  system  noise,  the  filter  is  still  able 
to  track  the  u  and  v  positions  of  the  target  as  well  as  provide  reasonable  estimates  of  the 
velocity  and  heading  despite  the  noticeable  non-linearity  of  the  target  variables.  However, 
performance  for  both  PKF-A  and  PKF-B  worsens  compared  to  Scenario  1.  Although  the 
position  estimation  is  still  reasonable  for  both,  estimates  of  velocity  and  heading  worsen 
due  to  their  non-linearity.  Furthermore,  the  errors  within  velocity  and  heading  adversely 
affect  the  position  estimation.  As  with  Scenario  1,  the  position  estimation  performance  of 
PKF-B  is  worse  than  the  performance  of  PKF-A  due  to  PKF-B ’s  erroneous  control  law. 
The  mean  errors  for  the  last  50  time  steps  are  provided  in  Table  3.5. 


Table  3.5:  Mean  Errors  without  System  or  Measurement  Noise 


Metrics 

u 

V 

V 

e 

PKF-A 

9.0307 

49.6103 

14.2988 

51.8838 

PKF-B 

20.2419 

16.2025 

11.2875 

50.3598 

PPF-B 

4.2103 

5.3242 

2.9838 

4.5664 

3.4.53  Scenario  3:  System  Noise  and  Measurement  Noise. 

.  Scenario  3  contained  system  noise  and  measurement  noise  in  the  target  model.  While 
the  filter  seeks  to  track  changes  in  the  target  due  to  system  noise,  the  filter  should  not  track 
changes  due  to  measurement  noise,  but  instead  filter  measurement  noise  to  determine  and 
track  the  actual  states.  Both  system  and  measurement  noises  were  identical  in  the  target  and 
filter  models.  Table  3.6  contains  the  system  and  measurement  variances  used  for  Scenario 
3. 
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Table  3.6:  PPF-B  Scenario  3:  Variances 


Variance 

o-{yuf 

o-ijvf 

cr  (xvf 

cr(xgf 

PKF-A 

10 

10 

1 

5 

PKF-B 

10 

10 

1 

5 

PPF-B 

10 

10 

2 

10 

System  variance  values,  cr{xvf'  and  crixef',  for  PKF-B  were  greater  in  order  to 
introduce  additional  variety  due  to  the  lack  of  an  acceleration  parameter  for  PKF-B.  The 
mean  errors  were  calculated  and  can  be  seen  in  Figure  3.6.  The  results  from  a  single 
simulation  run  are  provided  in  Figure  3.7.  An  important  note  is  that  since  the  system  noise 
directly  affects  the  subsequent  target  state  calculation,  each  simulation  will  be  different. 
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Figure  3.6:  PPF-B:  Mean  Error  for  System  and  Measurement  Noise 
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Figure  3.7:  PPF-B:  Single  Run  for  System  and  Measurement  Noise 
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As  seen  in  Figure  3.7,  with  the  presence  of  measurement  noise,  all  filter  estimations 
become  less  accurate.  In  particular,  over  time,  PPF-B  position  estimates  are  worse  than 
those  of  PKF-A  or  PKF-B  however  PPF-B  still  returns  notably  better  estimates  of  velocity 
and  heading.  Furthermore,  since  acceleration  values  change,  neither  control  law  is  accurate 
and  is  particularly  evident  again  for  velocity  and  heading  estimates.  The  differences  for  the 
hidden  states,  velocity  and  heading,  illustrate  a  key  benefit  of  the  particle  filter;  the  particle 
filter  does  not  require  knowledge  or  assumptions  concerning  acceleration.  The  overall 
mean  values  for  each  filter  are  provided  in  Table  3.7. 


Table  3.7:  Mean  Errors  without  System  or  Measurement  Noise 


Metrics 

u 

V 

V 

e 

PKF-A 

8.4716 

14.7124 

18.4685 

209.1750 

PKF-B 

7.6093 

11.7084 

7.8265 

209.0449 

PPF-B 

15.0517 

13.8954 

3.3029 

36.0236 

3.4.5.4  Scenario  4:  Weight  Discrepancy  and  Collapse. 

.  Scenario  4  illustrates  a  weight  collapse.  Table  3.8  contains  the  measurement 
covariances  used  to  produce  the  weight  collapse. 


Table  3.8:  PPF-B  Scenario  2:  Variances 


Variance 

O-ijuf 

o-{yvf 

Target 

1000 

1000 

PPF-B 

10 

10 

By  choosing  measurement  covariances  for  the  target  that  are  greater  than  those  for  the 
filter  ensures  that  some  measurement  points  will  lie  outside  the  distribution  used  by  the 
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filter.  The  filter  will  not  generate  the  correct  probability  that  the  state  is  correct  given 
the  measurement.  The  filter  assumes  the  measurements  are  more  accurate,  and  hence 
representative  of  the  target  states,  than  they  actually  are.  Although  the  filter  will  still  use 
the  most  heavily  weighted  and  thus  correct  particle,  the  total  importance  weight  summation 
begins  to  approach  zero.  Eventually,  none  of  the  particles  have  significant  weights  resulting 
in  filter  collapse  as  seen  in  Figure  3.8. 
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Figure  3.8:  PPF-B:  Weight  Diserepaney  and  Collapse 
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Towards  the  end  of  the  ’u  Position’  subplot,  PPF-B  begins  to  diverge  from  the  target. 
This  illustrates  PPF-B ’s  inability  to  generate  and  sample  particles  from  a  distribution  that 
contains  the  target  solution.  Figure  3.9  shows  the  distribution  of  particles  versus  the 
corresponding  target  state. 
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Figure  3.9:  PPF-B:  Iteration  Prior  to  Weight  Collapse 


400 
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As  seen  in  the  first  subplot,  U  vs  V  Position  Partiele  Weight,  the  target  state  lies 
well  outside  of  the  partiele  distribution.  Naturally,  PPF-B  assigns  the  greatest  weight  the 
elosest  partiele,  but  the  overall  weight  values  are  exeeptionally  small.  The  eollapse  oeeurs 
on  the  subsequent  iteration  when  the  weights  are  indistinguishable  from  zero,  meaning 
PPF-B  eannot  seleet  any  partiele.  These  eollapses  ean  be  mitigated  by  inereasing  the  filter’s 
measurement  noise  eovarianee  to  equal  or  exeeed  the  target  measurement  noise  eovarianee. 
If  the  filter’s  measurement  noise  eovarianee  exeeeds  that  of  the  target,  the  filter  ean  still 
traek  the  target  assuming  a  suflieient  number  of  partieles.  An  inerease  in  measurement 
noise  eovarianee  results  in  an  inerease  in  partiele  distribution.  A  larger  partiele  distribution 
results  in  an  inerease  in  the  number  of  partieles  with  an  importanee  weight  than  would 
be  justified  by  the  aetual  target  measurement  noise.  In  essenee,  the  filter  keeps  partieles 
that  it  should  diseard  due  to  the  inaeeurate  importanee  weights.  Thus,  in-order  to  deerease 
eomputational  eosts,  noise  eovarianees  should  ideally  be  equal  for  both  the  target  and  filter. 
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IV.  Methodology:  Application  Filters 


4.1  Introduction 

This  chapter  details  the  filter  development  philosophy  as  well  as  the  methods  used  to 
evaluate  the  performance  of  the  particle  filter. 

4.2  System  Design  Approach 

Proceeding  forward  based  on  positive  results  from  the  prototype  filters  discussed  in 
Chapter  2,  the  subsequent  particle  filters,  EPF-A  and  EPF-B,  attempt  to  track  a  target 
moving  non-linearly  in  3-D  space.  Before  developing  the  filters,  the  motion  of  the  target 
must  be  modeled  in  the  three  frames  discussed  in  Secion  4.2.1:  the  global  frame,  the  camera 
frame,  and  the  pixel  frame.  Additionally,  a  measurement  model  will  supply  simulated 
camera  measurements  to  the  filters  using  the  target  model  as  a  basis.  EPF-A  will  be  applied 
to  the  camera  frame  and  EPF-B  will  be  applied  to  the  pixel  frame. 

4.2.1  Coordinate  Frames. 

In  order  to  relate  measurement  data  from  the  cameras  to  the  target’s  position  in  3-D 
space,  the  data  must  be  moved  through  a  series  of  coordinate  frame  transformations.  The 
steps  necessary  to  move  from  the  global  frame,  which  the  target  moves  in,  to  the  final  pixel 
frame  that  the  camera  uses  to  observe  the  target  are  determined  with  a  series  of  coordinate 
fame  transformations.  These  transformations  are  necessary  to  both  generate  simulated 
measurements  and  produce  the  particles  by  the  particle  filter.  The  relationship  between 
the  target  data  and  single  camera  is  simpler  than  the  relationship  for  two  cameras.  Both  the 
global  and  camera  frames  share  the  same  origin  simplifying  the  necessary  transformations. 
The  three  coordinate  frames  used  are  the  global  frame,  the  camera  frame,  and  the  pixel 
frame.  The  first  transformation  is  exclusive  to  the  MATEAB  environment  due  to  a 
difference  between  the  default  MATEAB  coordinate  frame  and  the  preferred  global  frame 
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used  by  imaging  systems.  To  differentiate  between  the  default  MATLAB  global  frame  and 
imaging  global  frame,  the  axis  labels  change  using  the  following  notation. 

^ 

Z\  Zq 

The  default  positive  axis  directions  are  X\  up,  Yi  right,  and  Zi  into  the  page.  The  customary 
positive  axis  directions  for  image  processing  are  right,  Yq  down,  and  into  the  page. 
Thus,  a  90°  rotation  about  the  Z  axis  is  performed,  see  Figure  4.1. 


Xi 


,,  Yq 

Figure  4. 1 :  Global  Transformation  from  MATLAB  Default  Orientation 

The  second  transformation  is  from  the  global  to  the  camera  frame,  that  is,  the  frame 
aligned  with  the  camera.  The  axis  labels  change  using  the  following  notation. 

Xg  U\  ^  Uc 

Yg^V,^  Vc 
Zg^W,^  ITc 
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This  is  how  one  would  perceive  the  global  frame  if  rotating  and  panning  with  the  camera. 
The  two  angles  necessary  for  the  transformation  are  a  pan,  a,  about  the  Yg  axis,  and  a  tilt, 
jS,  about  the  Wi  axis,  see  Figure  4.2. 


Figure  4.2:  Transformation  from  Global  to  Camera  Frame 


These  two  transformations,  from  the  global  to  the  camera  frame,  can  be  described 
using  a  Direction  Cosine  Matrix  (DCM),  see  Equation  4.1.  All  the  rotations  detailed  thus 
far  are  about  one  of  the  three  orthogonal  dimensions,  Xi,  Yi,  and  Z\. 


cos{a  -  90)  0  -  sinCa  -  90)  1 


0 


0 


0  1  0 
sin(a;  -  90)  0  cos(a  -  90) 
Y  Rotation 


0  cos(J3  -  90)  -  sinljS  -  90) 
0  sin(/3  -  90)  cos(j3  -  90) 
X  Rotation 


cos(90)  -sin(90)  0 
sin(90)  cos(90)  0 
0  0  1 
Z  Rotation 


49 


The  final  transformation  is  from  the  eamera  frame  to  the  pixel  frame,  that  is,  the  three 
dimensional  target  loeation  is  projeeted  onto  two  dimensions.  This  is  aeeomplished  by 
dividing  eaeh  Uc  and  Vc  eoordinate  by  the  eorresponding  Wc  eoordinate,  see  Equation  4.2. 

[up,  vp]  =  —  [uc,  Vc]  (4.2) 

W2 

Figure  4.3  depiets  the  transformation  from  the  eamera  to  pixel  frame. 


Figure  4.3:  Transformation  from  Camera  to  Pixel  Frame 


As  seen  in  Figure  4.3,  if  an  objeet  is  viewed  without  knowing  any  absolute 
measurements,  it  is  impossible  to  determine  the  size  or  depth  of  that  objeet  onee  it  is 
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projected  onto  a  two-dimensional  image  plane.  This  is  the  point  at  which  depth  information 
is  lost  for  a  single  camera,  since  wc  cannot  be  redetermined  from  up  and  vp  alone. 

4.2.2  Target  Model. 

Based  upon  reference  frames  in  Section  4.2.1,  a  second  order  model  of  the  target 
was  developed.  The  equations  governing  the  motion  of  the  target  model  are  detailed  in 
Section  4.2.4.  The  target  model  generates  the  movements  of  a  single  point  representing  the 
centroid  of  the  target.  The  target  is  assumed  to  move  within  a  three  dimensional  space,  with 
a  position  of  XT,yT,  and  zp,  and  a  velocity  magnitude  of  Vp  defined  in  the  three  dimensional 
space  with  the  angles  6p,  tilt  from  the  Y  axis,  and  (pp,  pan  in  the  X-Z  plane,  as  shown  in 
Figure  4.4. 


Figure  4.4:  Target  in  Global  Frame 


The  states  that  describe  the  position  and  motion  of  the  target  in  the  global  frame  are 
provided  in  Table  4.1. 
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Table  4.1:  Global  Frame  Target  States 


Parameter 

Target  States 

Position 

XT 

Jt 

Zt 

Motion 

Vt 

6t 

4>t 

Position  Changes 

Axt 

Ajt 

Azt 

Motion  Changes 

AVt 

A6t 

Rates  of  Position 

Xt 

Jt 

Zt 

Rates  of  Motion 

Vt 

6t 

(pT 

4.2.3  Measurement  Model. 

Using  the  point  generated  by  the  target  model  as  a  basis,  the  measurement  model 
generates  additional  points  around  the  target  model  that  move  in  relation  to  the  target 
model,  simulating  a  three-dimensional  object  moving  in  three-dimensional  space.  The 
purpose  of  generating  additional  points  is  to  simulate  the  noise  that  may  be  introduced  when 
the  model  attempts  to  determine  the  motion  of  the  centroid  based  on  these  additional  points. 
Eight  measurement  points  were  generated,  forming  a  cube  around  the  target  centroid  as 
shown  in  Figure  4.5. 
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4.2.4  Global  Frame  Equations. 

The  motion  of  the  target  in  the  global  frame,  and  indeed  all  frames,  is  governed  by  the 
ehanges  in  V  and  headings  6  and  0.  These  variables  define  the  position  of  the  target  in  the 
global  frame  using  the  states  x,  y,  and  z.  Additionally,  the  motion  variables  are  impaeted  by 
their  respective  rates  of  change,  Vj,  Or,  and  The  target  model  is  a  second-order  discrete 
time  system,  derived  by  evaluating  the  continuous  system  at  a  time  At  and  eliminating  all 
terms  that  greater  than  second-order.  The  knowledge  of  acceleration  is  what  allows  the 
possibility  of  determining  depth,  discussed  further  in  Section  4.2.7  and  4.4.  The  position 
of  the  target  is  defined  by  Equation  4.3  through  4.5. 


XT,t  -  Xt4-\  +  AxT,t-l 

(4.3) 

yT,t  =  yT,t-i  +  ^yT,t-i 

(4.4) 

II 

T 

+ 

> 

T 

(4.5) 

Axt,  Ayr,  and  Azt  reflect  the  change  in  position  with  each  time  step.  The  changes 
in  position  are  determined  by  the  changes  in  V  and  headings  6  and  (p  and  defined  by 
Equation  4.6  through  4.8. 
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AxT,t  =  Vj^t  •  •  cos(0r,j)  •  At 

Af  .  Af' 

+VT,t  •  smiOrj)  ■  cos(07’,f)  •  —  +  eT,t  ■  Vj^t  ■  cosC^T-.f)  •  cos(^T,t)  ■  -Y 

Af' 

-<pT,t  •  VT,t  •  sm(eT,,)  ■  sm(^T,t)  ■  Yf 

Af  .  .  Af 

^yT,t  =  yT,t  •  cos(07’,)  •  At  +  Vr^t  •  cosiOjf  •  —  ^T,t  •  •  sin(07’f)  •  ^- 


(4.6) 

(4.7) 


Azr.r  =  V  ■  sin(dT,t)  ■  sin(^T,t)  ■  A? 

Af  .  Af 

+VT,t  ■  smiOrj)  ■  sm(^T,t)  ■  ^  +  (pT,t  •  Vt,,  •  smiOrf  ■  cos(^T,t)  ■  ^ 

Af 

+(pT,t  •  Vtj  •  Sin(07’ ,)  •  C0S(^7’  ()  • 


(4.8) 


The  velocity  and  headings  at  each  time  step  are  defined  by  Equation  4.9  through  4.1 1. 


1 

< 

+ 

1 

II 

(4.9) 

II 

T 

+ 

> 

T 

(4.10) 

II 

T 

+ 

> 

T 

(4.11) 

The  changes  in  velocity  and  heading  from  each  time  step  are  defined  by  Equation  4.12 

through  4.14. 

< 

II 

< 

(4.12) 

A6tj  —  6tj  •  At 

(4.13) 

AOj^t  =  9j^t  ■  At 

(4.14) 

The  model  assumes  that  6Vt,  59t,  and  are  constant  since  acceleration  is  held  constant. 

Thus,  the  changes  in  velocity  and  heading  must  be  constant  as  well. 

Equation  4.15  through  4.17  which  define  V,  9,  and  0. 

This  is  reflected  in 
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Vtj  =  6VT,t-i^t 

(4.15) 

Otj  =  SOrj-i/^t 

(4.16) 

(4.17) 

4.2.5  Camera  Frame  Equations. 

As  discussed  earlier,  the  resulting  states  must  be  first  be  transformed  to  the  camera 
coordinate  frame  using  the  rotation  matrices  specified  in  Equation  4.1.  Both  the  target 
centroid  and  measurement  points  were  transformed  in  this  fashion.  These  transformations 
are  described  by  Equation  4.18  and  4.19. 


XT 

Utc 

pT 

^DCM 

yr 

= 

VTc 

Zt 

Wtc 

Xt 

Utc 

^DCM 

yr 

= 

VTc 

Zt 

Wtc 

(4.18) 


(4.19) 


4.2.6  Pixel  Frame  Equations. 

The  final  transformation  is  to  the  pixel  coordinate  frame  so  that  the  filter  predicted 
measurements  may  be  compared  to  actual  measurements.  The  transformation  results  in 
a  conversion  from  the  three  dimensional  camera  coordinate  frame  to  the  two  dimensional 
pixel  frame  that  is  representative  of  what  the  camera  would  actually  see.  The  resulting 
states  are  illustrated  by  Equation  4.20. 


Utc 

Utp 

Utc,  WTc 

Utp 

VTc 

VTp 

VTc,  Wtc 

VTp 

Wtc 

St 

(4.20) 
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The  measurement  states  for  the  target,  ujp,  vjp,  sj,  utP,  and  vjp  are  defined  by 
Equation  4.21  through  4.25,  where  fl  is  the  focal  length  of  the  camera). 


Utp  - 

Utc 

■fl 

(4.21) 

Wtc 

VTp  = 

VTc 

■fl 

(4.22) 

Wtc 

1  / 

-Utc^Wtc  -VTc^WtA 

(4.23) 

St  = 

■2'V 

2  +  2 
^Tc  ^Tc  1 

iiTp  = 

Utc 

■fl 

(4.24) 

Wtc 

VTp  = 

VTc 

■fl 

(4.25) 

Wtc 

The  focal  length  fl  adds  a  dimension  to  an  otherwise  unitless  measurement,  the  pixel.  The 
focal  length  also  determines  the  camera’s  angle  of  view.  Longer  focal  lengths  correspond 
in  smaller  view  angles  while  shorter  focal  lengths  correspond  with  larger  view  angles,  as 
seen  in  Figure  4.6  [9].  Additional  properties  are  provided  in  Table  4.2. 
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Camera  Lens 


Image  Sensor 


< - > 

Focal  Length 


Figure  4.6:  Relationship  Between  Foeal  Length  and  View  Angle 


Table  4.2:  Image  Properties 


Focal  Length 

View  Angle 

Area  Captured 

Apparent  Size 

Short 

Wide 

Large 

Small 

Long 

Small 

Small 

Large 

The  focal  length  also  allows  the  calculation  of  angle  error.  Given  a  focal  length,  /,  and 
the  difference  between  the  filter  estimated  location  of  the  target,  u fp  and  actual  location  of 
the  target,  ujp,  an  angular  error  y  can  be  determined.  This  provides  another  useful  metric 
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to  compare  the  particle  filter’s  performance  across  different  systems. 

arctan|^^^^^^^^j  =  y  (4.26) 

4.2. 7  Acceleration  and  Depth  Knowledge. 

Since  the  pixel  measurements,  Up,  Vp,  w.  Up,  and  Vp,  are  based  on  angle  measurements, 
position  estimates,  such  as  depth,  are  impossible  to  determine  without  knowledge  of  an 
initial  length  scale.  However,  if  the  target’s  acceleration  is  non-zero  and  known,  then  the 
length  scale  can  be  determined  by  integrating  acceleration  to  determine  velocity  and  the  by 
integrating  velocity  to  determine  acceleration.  If  the  known  acceleration  is  zero,  no  length 
scale  can  be  determined  since  integrating  results  in  a  trivial  solution.  The  results  for  EPF-B 
contained  in  5  demonstrate  the  validity  of  requiring  a  non-zero  acceleration. 

4.3  Evaluated  Particle  Filter  A 

EPF-A  uses  the  camera  measurements,  Uc,  Vc,  and  Wc,  to  track  the  target.  This  filter 
simulates  the  data  that  might  be  generated  by  two  cameras,  or  a  system  with  a  range 
finding  device  such  as  EIDAR.  Additionally,  EPF-A  will  characterize  the  performance  of 
the  particle  filter  tracking  multiple  hidden  states  in  three  dimensions.  The  hidden  states  are 
those  that  cannot  be  directly  observed.  For  EPF-A,  the  hidden  states  are  V,  6,  and  (j).  The 
steps  taken  by  the  filter  correspond  to  those  in  Section  2.5.5  and  are  subsequently  discussed 
in  specific  detail. 

4.3.1  Initialization  and  Proposal  Distribution. 

EPF-A  uses  the  same  model  as  the  target  to  both  generate  the  initial  and  subsequent 
proposal  distributions  and  rotate  to  the  camera  frame.  The  key  difference  is  that  at  the  re¬ 
sampling  step,  the  filter  adds  the  system  noise  variance  to  generate  the  proposal  distribution. 
Equation  4.27  through  4.29  show  the  states  and  manner  in  which  variety  is  added  at  the 
sampling  step.  r„  is  a  random  number  sampled  from  a  standard  uniform  distribution 
between  0  and  1.  However,  any  distribution  may  be  used  to  generate  the  variety;  the 
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distribution  is  not  required  to  be  a  white  Gaussian  distribution. 


II 

T 

-1- 

> 

T 

-1- 

■  r„ 

(4.27) 

II 

T 

-1- 

> 

T 

+ 

Jo-^(xf,e)  ■  r„ 

(4.28) 

II 

T 

-1- 

> 

T 

-1- 

yJ(T^(Xf-f)  ■  Pu 

(4.29) 

Variety  was  only  added  to  Vf,  Of,  and  (pf  since  the  positions  are  derived  from  these  states 
and  it  is  the  positions  that  are  compared  against  the  measurements  once  transformed  into 
the  appropriate  frame.  If  variety  were  added  directly  to  the  states  Xf,yf,  and  Zf,  this  would 
not  only  be  redundant,  but  also  adversely  affect  the  determination  of  the  hidden  states  since 
the  particle  filter  may  select  measurements  that  no  longer  directly  corresponding  to  the 
hidden  states  that  generated  them.  The  necessary  rotations  are  then  performed  to  produce 
the  camera  frame  position  states,  ucj,  vcj,  and  wcj,  for  these  particles. 

4.3.2  Importance  Sampling. 

As  discussed  in  Section  2.5.5,  the  particle  filter  must  sample  importance  weights  from 
the  relative  posterior  PDF  p  j.  EPF-A  uses  the  same  distribution  model  as  PPF-B,  that 
is  a  normal  distribution,  discussed  in  Section  3.4.3  and  detailed  in  Equation  3.15.  Thus, 
the  variables  that  must  be  set  are  the  weighing  matrix,  W,  and  the  noise  variance  in  the 
measurement,  cr^  (m).  Models  containing  multiple  measurements  can  utilize  a  weighing 
matrix  when  calculating  the  importance  weight  for  each  particle.  This  allows  for  the  model 
to  account  for  the  quality  of  each  measurements;  measurements  that  are  known  to  be  noisier 
than  others  may  be  assigned  a  lower  weight  within  the  weighing  matrix  and  vice-versa. 
Equation  4.30  defines  the  weighing  matrix  used  by  EPE-A. 


Q.{u)  0 


•EPF-A 


0  D(v) 


0 

0 


0  0  D(w) 


(4.30) 


The  importance  weights  are  subsequently  normalized  using  Equation  2.25. 
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4.3.3  Resampling  and  Global  State  Estimation. 

EPF-A  uses  the  re-sampling  algorithm  outlined  in  Seetion  2.5. 5. 4.  The  re-sampling 
step  generates  the  approximation  of  the  posterior  filtering  distribution  p(Xf|yi.().  The 
estimated  global  states  are  determined  by  taking  an  average  mean  of  the  posterior  filtering 
distribution. 

4.3.4  Preliminary  Evaluation. 

Before  eondueting  a  battery  of  evaluation  tests,  a  single  test  was  performed  to  ensure 
EPF-A  is  able  to  funetion.  Initial  eonditions  for  the  target  and  filter  differ,  foreing  the  filter 
to  loeate  and  traek  the  target,  and  measurement  noise  was  added.  Due  to  eonfidenee  in  the 
performanee  of  this  filter,  the  target  and  filter  used  differing  initial  eonditions  to  determine 
if  the  filter  eould  traek  to  the  target.  The  filter  is  eompared  against  the  Simple  Einear  Model 
A  (SEMA)  and  Evaluation  Kalman  Filter  A  (EKF-A),  both  eomparison  filters  developed 
in  Seetion  5.2.2. 1  and  5. 2.2. 3  respeetively.  Additional  tests  and  performanee  eriteria, 
ineluding  hidden  state  evaluation,  is  diseussed  in  Chapter  5.  Table  4.3  and  4.4  eontains  the 
initial  eonditions  and  measurement  varianees.  The  results  are  plotted  in  Figure  4.7  and  4.8. 


Table  4.3:  Filter  A  Preliminary  Evaluation  Initial  Conditions 


States 

X 

z 

V 

e 

0 

Ax 

Ay 

Az 

AT 

A0 

A0 

Target 

5 

5 

5 

1 

45 

45 

0 

0 

0 

0.1 

5 

5 

Filter 

0 

0 

10 

0 

0 

0 

0 

0 

0 

0 

0 

0 

Filter  Varianee 

1 

1 

1 

1 

0.1 

0.1 

0.5 

0.5 

0.5 

0.1 

0.01 

0.01 
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Table  4.4:  Filter  A  Preliminary  Evaluation  Measurement  Noise  Variances 


States 

u 

V 

w 

Target 

5 

5 

5 

Filter 

5 

5 

5 

As  seen  in  Figure  4.7,  the  filter  appears  to  track  the  target  successfully  according  to 
the  metrics  developed  in  Section  5.2.1.  Additional  tests  and  performance  criteria,  including 
hidden  state  evaluation,  is  discussed  in  Chapter  5.  Figure  4.8  demonstrates  EPF-A’s  ability 
to  track  the  hidden  states  more  accurately  than  either  SFMA  or  EKF-A.  Table  4.5  provides 
the  mean  values  for  the  last  50  time  steps. 


Table  4.5:  Filter  A  Preliminary  Evaluation:  Mean  Errors 


Metrics 

X 

z 

V 

e 

0 

SFMA 

4.1725 

4.2698 

4.0238 

97.3539 

44.7735 

121.9448 

EKF-A 

2.5025 

6.1966 

1.9997 

37.7595 

88.7395 

56.9636 

EPF-B 

1.6095 

1.2216 

1.7188 

1.9931 

16.6655 

51.0877 

4.4  Evaluated  Particle  Filter  B 

EPF-B  is  nearly  identical  to  EPF-A  except  that  it  uses  the  pixel  measurements  as 
measurements;  that  is  the  pixel  location,  Ufp  and  v/p,  and  the  velocities,  Sf,  iifp,  and  v/p,  all 
defined  by  Equation  4.33  through  4.37.  These  pixel  measurements  are  based  on  the  pixel 
location  of  the  target  centroid.  In  contrast,  the  camera  pixel  measurements  are  eight  sets  of 
pixel  measurements,  one  for  each  of  the  observed  comer  points  of  the  target.  These  eight 
camera  pixel  measurements  must  be  condensed  to  a  single  set  of  camera  measurements 
that  describe  the  centroid.  The  averages  of  the  eight  pixel  positions,  Yjm=iiiTpt-i 
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Position  Position  Position 


X  Position 


Figure  4.7:  Filter  A  Preliminary  Evaluation:  Position 
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Heading  (deg)  Heading  (deg)  Velocity 


Velocity  Magnitude 


Figure  4.8:  Filter  A  Preliminary  Evaluation:  Velocity 
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Zm=i^rpf-i’  assumed  to  provide  the  representative  values  of  the  centroid  location 
which  are  used  as  the  measurement  states  Up  and  Vp.  Least  mean  squares  determined  the 
measurement  states  sj,  utP,  vtP,  at  each  time  step  t,  using  an  M  number  of  matched  points. 
Equation  4.31  and  4.32  detail  the  steps  taken  to  determine  the  measurement  states. 

Ip 


itpt  V pt 

Dimensions  [1x3] 

Labels  A 


0...0  At. .At 

At. ..At  0...0 

[3  X  2M] 

X 


Aij^  Aij^  Av^  Av^ 

l\UTpf...lXUTpf 

[1  X  2M] 
b 


A  =  b  •  x'^  ■  (x  ■  x'^)  ^ 


(4.31) 

(4.32) 


Thus,  EPF-B,  mirroring  the  target  model,  predicts  the  pixel  measurements  expected  for 
each  particle  using  Equation  4.33  through  4.37. 


UpJ  - 


Vpj  = 


^f  = 

bpj-  = 

V/  = 


^c,f 

Wc.f 

^c.f 

Wc.f 

Wc.f 

Wfc 


■fl 


■fl 


■fl 


Uc,f 

Wc.f 

I’c.f 

Wc.f 


■fl 


■fl 


Additionally,  the  weighting  matrix  changes  slightly  as  seen  in  Equation  4.38. 


a 


EPF-B 


Q.(Upj)  0  0 

0  Q(vpj)  0 
0  0  ^(Sf) 


0 

0 

0 


0 

0 

0 


0 

0 


0 

0 


0 

0 


Qiiipj)  0 
0  ^(vp,f) 


(4.33) 

(4.34) 

(4.35) 

(4.36) 

(4.37) 


(4.38) 
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4.4.1  Preliminary  Evaluation. 

As  with  EPF-A,  EPF-B  underwent  a  preliminary  test  to  evaluate  its  functionality.  The 
first  evaluation  was  movement  along  the  x-axis  at  constant  velocity,  with  no  measurement 
noise,  and  the  following  initial  conditions,  seen  in  Table  4.6. 


Table  4.6:  Filter  B  Preliminary  Evaluation  Initial  Conditions 


States 

X 

z 

V 

e 

0 

Ax 

Ay 

Az 

AV 

Ae 

A0 

Target 

0 

0 

5 

1 

90 

0 

0 

0 

0 

0 

0 

0 

Filter 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

Filter  Variance 

0.1 

0.1 

0.1 

0.5 

0.5 

0.5 

0.1 

0.1 

0.1 

0.1 

0.1 

0.1 

The  simulation  used  500  particles  and  a  measurement  noise  covariance  of  30.  Within 
5  time  steps,  EPF-B  crashed  due  to  weight  collapse.  The  last  sum  of  weights  was  equal 
to  3.307  X  10“^^°.  Additional  simulation  runs  resulted  in  similar  results.  Weight  collapse, 
as  discussed  in  2.5.6,  can  be  mitigated,  typically  with  the  addition  of  particles.  Multiple 
subsequent  evaluation  were  performed,  using  1500  particles,  and  these  too  resulted  in 
weight  collapse.  Figure  4.9  illustrates  particle  collapse  from  the  perspective  particle 
distribution. 


65 


u  vs  V  Position  Particle  Weight  x  vs  y  Position  Particle  Weight 


u  vs  V  Velocity  Particle  Weight 


X  vs  y  Velocity  Particle  Weight 


Figure  4.9:  Particle  Filter  B  Weight  Collapse 


Although  there  still  exists  a  spread  of  particles,  none  have  sufficient  weight  to  be 
sampled.  Part  of  this  may  stem  from  insufficient  variance.  As  seen  in  the  first  subplot, 
entitled  U  vs  V  Position  Particle  Weight,  the  target  value  lies  outside  the  spread  of  particles 
and  hence,  cannot  be  sampled.  The  subsequent  sections  detail  the  methods  taken  that 
attempt  to  mitigate  weight  collapse. 

4.5  Weight  Collapse  Mitigation 

One  of  the  core  concepts  of  the  particle  filter  is  variance  and  the  spread  of  particles 
throughout  the  state  space  to  include  the  target  solution  within  the  particle  spread.  As 
demonstrated  in  Section  4.4.1,  the  present  form  of  EPF-B  does  not  include  the  solutions 
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within  its  particle  spread.  In  order  to  encompass  the  solution,  variance  must  be  in 
such  a  manner  that  expands  the  particle  spread  in  the  direction  of  the  likely  solution 
and  not  in  a  manner  that  simply  adds  more  variation  in  all  directions.  Two  methods 
were  explored;  variation  along  the  depth  vector,  referred  to  as  the  Depth-Compensated 
Evaluation  Particle  Filter  B  (DC),  and  variation  dependent  on  the  relationship  between  in 
hidden  and  observable  states,  referred  to  as  the  Jacobian-Compensated  Evaluation  Particle 
Filter  B  (JC).  One  further  change  was  also  made  to  the  weighing  matrix  for  the  importance 
weights. 

4.5.1  Depth  Vector  Variation. 

The  DC  is  based  on  the  unit  vector  of  the  target  centroid  as  observed  from  the  camera. 
Since  the  camera  cannot  detect  depth,  the  ucj  and  vcj  variables  vary  in  proportion  to  wcj 
with  the  same  Upj  x  Vpj.  The  variance  factors  are  based  on  s  (the  measurement  of  depth 
over  time)  while  maintaining  good  measurements  of  heading.  One  of  the  drawbacks  of  this 
variation  method  is  that  it  introduces  variation  into  the  position  states.  The  position  states 
are  derived  from  the  hidden  variables  and  by  introducing  variation  directly  into  the  position 
states,  the  DC  will  affect  the  accuracy  of  the  hidden  states  since  a  particle  will  be  selected 
based  on  combination  the  hidden  state’s  values  and  this  injected  variation. 

4.5.2  State  Jacobian  Variation. 

The  JC  avoids  the  adverse  influence  on  the  position  states  by  only  introducing  variation 
into  the  hidden  states  directly.  The  JC  attempts  to  determine  the  appropriate  variance 
values,  cr{xf^(^,  for  each  time  step  based  on  the  amount  of  influence 

the  hidden  states,  Vf,  Of,  and  will  have  on  the  measurements  Sf,  Upj,  and  Vpf.  The 
reason  these  measurements  were  chosen  is  that  they  are  the  least  certain  and  they  are 
exclusively  derived  from  the  three  hidden  states  mentioned.  A  first  order  approximation 
of  Equation  4.6  through  4.8  is  used  to  evaluate  the  sensitivity  of  the  measurements  to  the 
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states  by  Equation  4.39,  4.40,  and  4.41. 


Xf,t  =Vf^t  •  sin(0/,f)  •  sm((f)fj)  ■  At 

(4.39) 

yf,t  =Vf,t-  cosiOfj)  ■  At 

(4.40) 

if  =V  ■  sin(0/,j)  •  sin(0/,f)  •  At 

(4.41) 

The  velocity  approximations  are  the  transformed  to  the  camera  oriented  coordinates  by  by 
Equation  4.18.  The  influence  of  the  hidden  states  on  the  measurements  can  be  determined 
by  examining  the  eigenvalues  and  eigenvectors  that  describe  the  correlation  between  these 
variables.  The  general  formula  describing  how  the  change  in  states,  AX,  affects  the  change 
in  measurement  states,  A5 ,  is  described  by  Equation  4.42. 

A5  =  — AA  (4.42) 

oX 


The  eigenvalues  and  eigenvectors  are  drawn  from  the  ||.  Since  the  hidden  states  pass 
through  several  transformations  within  EPE-B  before  being  used  to  generate  measurements, 
a  combination  of  Jacobian  and  rotation  matrices  are  used,  described  and  expanded  in 
equations  4.43,  4.44,  and  4.45.  All  states  are  based  on  the  individual  particle  state 
estimates. 
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(4.43) 


dx  dx  dx 

dv  do  d(p 
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dv  do  d(p 

dz  dz  dz 
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(4.44) 
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0  0  4 
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cos0sin0At  -Vsin^sin^At  V  cos  9  cos  (f> At 
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(4.45) 
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The  eigenvectors,  V,  describe  the  direction  and  relative  sensitivity  of  each  factor.  In  other 
words  if  the  measurement  would  be  positively  or  negatively  affected  by  each  state  and  in 
what  proportion.  The  eigenvalues,  A,  describe  the  amount  of  influence  for  each  eigenvector. 
The  measurement  state  variances  are  determined  using  the  eigenvectors  and  eigenvalues, 
along  with  scaling  values,  5/,  and  Sf^.  Sf^  scales  the  range  and  Sf^  sets  the  minimum 
variance. 

5/,  •  V  •  diag  (diag  (A)  +  s/,)  ^  (4.46) 

4.S.3  Weighing  Matrix  Adjustment. 

Of  the  five  pixel  measurements  uj^p,  vp^p,  sj,  UT,p,  and  vt,p,  the  filter  is  least  certain  of 
St  since  it  does  not  directly  correspond  to  an  actual  depth  variable.  Instead,  is  an  estimate 
dependent  on  wt,p  and  WT,p,  defined  in  Equation  4.25.  Since  wt,p  is  a  measurement  of 
velocity,  the  accuracy  of  the  depth  estimation  might  be  increased  by  increasing  the  weight 
of  the  three  velocity  measurements,  UT,p,  vt,p,  and  sp.  The  reasoning  for  this  proposal  is 
that  by  weighting  the  velocities  higher,  less  weight  will  be  placed  on  the  pixel  position 
since  the  pixel  position  is  easier  to  measure.  Weighting  the  velocities  places  an  emphasis 
on  depth  to  match  the  known  acceleration,  which  is  the  only  parameter  that  can  provide  an 
unambiguous  estimate  of  range.  This  proposed  adjustment  will  be  evaluated  in  Section  5.4. 
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V.  Simulation  Tests  and  Results 


5.1  Introduction 

Once  finalized,  both  EPF-A  and  EPF-B  were  evaluated  using  a  variety  of  seenarios. 
Due  to  the  dilferenees  in  the  filters,  eaeh  underwent  dilFerent  simulations.  All  seenarios 
were  eondueted  both  without  and  with  measurement  noise  to  simulate  realistie  noisy 
measurements.  Results  from  these  simulations  were  analyzed  using  various  metries  in  order 
to  evaluate  the  performanee  of  both  filters.  Both  filters  were  also  eontrasted  against  linear 
models,  SEMA  and  Simple  Einear  Model  B  (SEMB)  that  used  the  same  measurements 
provided  to  the  respeetive  filters.  The  details  of  these  metries  and  the  aeeompanying 
linear  eomparison  models,  SEMA  and  SEMB,  are  diseussed  in  Seetion  5.2.1  and  5.2.2 
respeetively. 

5.2  Particle  Filter  Performance  Evaluation 

Performance  metrics  must  be  produced  in  order  to  evaluate  the  viability  of  particle 
filters.  However,  particle  filters,  due  to  their  stochastic  nature,  pose  two  unique  challenges 
compared  to  linear  filters,  such  as  the  Kalman.  One  challenge  is  devising  performance 
metrics.  Due  to  their  stochastic  nature,  each  time  the  filter  executes,  the  resulting  estimates 
will  be  dilferent  given  the  same  target  model  and  measurements.  In  contrast,  a  linear  filter 
will  consistently  return  the  same  estimations  each  time  it  is  executed,  assuming  the  same 
measurements  are  provided  each  time.  In  order  to  develop  a  better  comparison  to  a  linear 
filter,  either  noise  must  be  introduced  to  change  the  measurements  or  the  states  themselves 
must  be  changed  either  by  introducing  system  noise  or  by  changing  the  initial  conditions. 
An  additional  challenge  is  devising  a  reasonable  comparison  to  compare  the  filter  against. 
Without  a  baseline  comparison,  it  is  difficult  to  quantify  how  well  the  filter  is  tracking  the 
target.  Although  the  filter  may  appear  to  estimate  the  target  states,  the  question  that  must  be 
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addressed  is  how  mueh  better  does  it  estimate  the  target  states  eompared  to  random  guesses 
of  the  target  states. 

5.2.1  Performance  Metrics. 

Sinee  eaeh  exeeution  of  the  partiele  filter  results  in  different  predietions,  any  proposed 
metrie  must  evaluate  the  filter’s  performanee  over  multiple  simulations.  With  this 
requirement  in  mind,  one  of  the  primary  questions  is  how  well  does  the  filter  prediet  the 
target  states,  in  partieular  the  hidden  states.  This  question  may  also  be  posed  as  what  is 
the  differenee,  or  error,  between  the  target  state  and  the  filter  estimate  of  that  state.  Two 
metries  were  devised  that  answer  this  question  in  different  manners:  the  Mean  Absolute 
Error  (MAE)  and  the  Threshold  Error  Range  (TER). 

5.2.1. 1  Mean  Absolute  Error. 

The  MAE  provides  a  mean  of  absolute  errors  between  the  target  states,  x,  and  filter 
estimate  of  those  states,  x/,  over  an  q  number  of  simulations  at  eaeh  time  step,  k. 


^MAE, 


1 


(5.1) 


The  MAE  shows  what  values  the  filter  eould  reasonably  be  expeeted  to  return,  aeeounting 
for  both  good  and  poor  estimations.  Additionally,  sinee  the  MAE  is  ealeulated  for  eaeh 
time  step,  speeifie  time  ranges  ean  be  evaluated;  for  instanee,  the  mean  estimates  when 
the  filter  is  traeking  to  the  target  or  how  mueh  varianee  eould  be  expeeted  between  the 
different  filters  when  eaeh  has  aequired  and  is  aetively  traeking  to  the  target.  A  drawbaek 
of  the  MAE  is  that  it  does  not  determine  the  proportion  of  aeeurate  estimations  to  poor 
estimations.  Eor  instanee,  a  reasonable  mean  may  be  returned  from  several  good  estimates 
and  a  handful  of  dismal  estimates.  These  dismal  estimates,  if  numerous  or  severe  enough, 
eould  invalidate  the  filter,  but  may  be  obseured  by  the  mean  and  never  notieed  otherwise. 
The  seeond  metrie,  the  TER  attempts  to  address  this  issue  by  determining  an  effeetive 
operating  range  of  the  filter. 
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5.2. 1.2  Threshold  Error  Range. 

The  TER  first  determines  the  absolute  error  between  the  target  states  and  filter 
predictions,  at  each  time  step,  for  each  simulation.  At  each  time  step,  these  error  values 
are  sorted  from  lowest,  representing  the  smallest  error,  to  highest,  representing  the  greatest 
error.  A  percentile  value,  r,  determines  the  ratio  of  error  values  to  return.  If  r  were  1, 
then  the  TER  would  always  return  the  maximum  error  found  from  all  simulations  at  each 
time  step.  If  r  were  0.5,  then  the  TER  would  return  the  error  value  for  which  half  of  the 
simulation  errors  were  equal  to  or  less  than  for  each  time  step;  in  other  words  the  median. 
Since  the  TER  is  not  a  mean,  it  is  a  better  predictor  or  extreme  behavior,  hence  the  term 
threshold.  If  a  r  of  0.9  is  selected,  then  for  the  error  returned  at  a  particular  time  step,  90% 
of  all  simulations  would  have  errors  less  than  or  equal  to  that  value  at  that  time  step.  The 
TER  determines  the  error  to  return  by  first  determining  the  appropriate  index  number  by 
multiplying  the  selected  percentile  value,  r,  by  the  number  of  simulations,  q.  The  errors  are 
sorted  lowest  to  highest  and  the  error  at  the  index  corresponding  to  the  percentile  value  is 
returned.  Equation  5.2  and  5.3  define  the  TER  calculation,  where  sort  is  a  function  ranking 
the  corresponding  errors  lowest  to  highest. 


XS(  =  sortlxr  -  x/|^ 

(5.2) 

^TER,t  =  XS,(rT-^l) 

(5.3) 

Eor  all  scenarios  detailed  in  Chapter  5,  a  threshold  of  0.9  was  used,  meaning  90%  of  all 
simulations  in  a  particular  scenario  had  errors  less  than  or  equal  to  the  error  value  returned 
by  TER.  Eigure  5.1  provides  a  visual  depiction  of  the  TER. 
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Figure  5.1:  Threshold  Error 


5.2. 1.3  Metric  Variables. 

The  previous  performance  metrics,  MAE  and  TER  evaluated  performance  based  on 
the  following  variables:  D,  the  Euclidean  distance  between  the  target  and  filter  prediction, 
V,  the  velocity  magnitude,  and  the  heading  angles,  6  and  (p.  D  is  defined  by  Equation  5.4. 
Eigure  5.2  provides  a  visual  depiction  of  the  metric  variables. 


(5.4) 
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Figure  5.2:  Performance  Metric  Variables 

Both  D  and  V  have  unlimited  error  ranges,  while  0  and  0  are  constrained  between  0° 
and  1 80°  since  the  greatest  possible  error  is  a  heading  in  the  opposite  direction. 

5.2.2  Comparison  Models. 

In  order  to  provide  reasonable  comparisons  for  the  particle  filter,  two  linear  models 
were  developed,  SLMA  and  SLMB,  to  serve  as  comparisons  for  EPF-A  and  EPF-B 
respectively.  Each  uses  the  same  measurements  provided  to  the  filter  it  is  compared  against, 
and  attempts  to  predict  the  target  states.  Additionally  a  Kalman  filter,  EKE-A  similar  to 
PKE-B,  was  developed  as  an  additional  comparison  for  EPE-A.  A  comparison  Kalman 
filter  was  not  developed  for  EPE-B  due  to  EPE-B’s  immaturity  compared  to  EPE-A  as 
discussed  in  Section  4.4  and  5.4. 

5.2.2. 1  Linear  Model  A:  Filter  A. 

The  SEMA  uses  the  measurements  provided  to  EPE-A,  which  are  u,  v,  and  w, 
to  predict  the  corresponding  global  values.  Since  only  position  is  measured,  velocity 
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is  approximated  using  a  first  order  backward  difference  approximation.  The  SLMA  is 
modeled  using  Equation  5.5  through  5.6. 


UlMj  — 
VlMj  = 


WlMa  -  Wlmj-I  + 


The  variables  are  then  rotated  from  the  camera  frame  to  the  global  frame  using  the  DCM 
from  Equation  4.1.  The  variable  transformations  are  provided  by  Equation  5.8. 


ULM,t  ^LM,t 

(Rdcm)  vlmj  yLM,t  (5-8) 

WLM,t  ZLM,t 

The  component  velocities,  Xlm,!,  Xlmj,  and  XLM,t  are  determined  using  Equation  5.9 

through  5.11. 


yLM,t  - 


XlMj  -  XLM,t-l 

At 

yLM,t  —  yLM,t-\ 

At 

ZLM,t  —  ZLM,t-l 

At 


(5.10) 


(5.11) 


The  hidden  states,  V  and  headings  0  and  (f>  can  now  be  estimated  with  the  estimated  global 
state  variables,  using  Equation  5.12,  5.13,  and  5.14. 


yLM,t  -  y^MJ  ^LMj 


Olmj  =  arccos 


(pLM,t  =  arctan 


(5.12) 


(5.13) 


(5.14) 


5.2.2.2  Linear  Model  B:  Filter  B. 


The  SLMB  is  similar  to  SEMA  regarding  the  calculation  of  estimated  states,  however 
SLMB  uses  the  measurements  Up,  Vp,  s,  tip,  and  Vp,  along  with  the  target  value  for  Wtgt^. 


75 


At  least  one  target  variable  must  be  used  since  the  SLMB  attempts  to  extract  3-D  variables 
from  Two-Dimensional  (2-D).  The  use  of  this  target  variable  is  minimized  to  prevent  the 
SLMB  having  too  significant  an  advantage  over  EPF-B. 


ULM,t 

VLM,t 

WLM,t 

Ulmj 

VLM,t 

WLM,t 


WlMj-I  •  UiMp,t 

7 

WLM,t-\  •  VLMp,t 

7 


WT,t  •  {-ULM,t  -  VlM.t) 

2  •  s 

ULMp,t  •  WLM,t 

7 

VLMp,t  •  WLM,t 

7 

WLM,t  -  WLM,t-\ 

At 


(5.15) 

(5.16) 

(5.17) 

(5.18) 

(5.19) 

(5.20) 


These  variables  are  then  rotated  from  the  camera  frame  to  the  global  frame  using  the 
inverse  DCM,  defined  in  Equation  4.1.  The  transformations  are  the  same  as  those  defined 
in  Equation  5.8.  The  hidden  states,  V  and  headings  6  and  (p  can  now  be  estimated  with 
these  rotated  variables  using  Equation  5.12  through  5.14. 

5.2.2. 3  Evaluate  Kalman  Filter  A. 

The  EKF-A  is  similar  to  PKF-A  and  PKF-B,  both  developed  in  Section  3.4.4,  but 
EKF-A  is  expanded  to  3-D  space  instead  of  2-D  space.  The  positional  states  are  .r,  y,  z, 
while  the  target  velocity  states  are  linearized  from  V,  6,  and  (p  to  the  component  velocities 
X,  y,  and  z  as  seen  in  Equation  5.21  through  5.23. 


i^k,t  - 

--Vuy 

■  sin  {ek,t)  ■ 

cos 

•  At 

(5.21) 

Vk,t  -- 

--Vky 

■  cos  (Okj) 

■  At 

(5.22) 

Wk,t  -- 

--V,y 

■  sin  {9kj)  • 

sin  ((pk,t)  • 

At 

(5.23) 
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The  component  velocities  are  transformed  back  to  velocity  magnitude  and  heading  via 
Equation  5.24  through  5.26  in  order  to  provide  a  direct  comparison  to  the  target  states. 


.-4 


x'k/  +  y'k/  +  Zk/ 


Ok,t 

Vkj  =  arccos  ]  — 

yk,t 
(Pk,t 


Wk,t  =  arctan 


(5.24) 

(5.25) 

(5.26) 


The  control  law,  Uk,  consists  of  the  component  accelerations,  x,  y,  and  z,  respectively. 
The  control  law  components  are  determined  from  the  initial  conditions  by  Equation  5.27 
through  5.29. 


V  Af  e  Af 

Uk,x  =  —  ■  sin(0)  •  cos(^)—  +  —  •  V  •  cos(0)  cos(^)— 
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2  At 
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At  2 


V  At^ 

Uk,y  =  —  ■  COS(6')  •  — 


e  Af 
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y  Af  6  Af 

=  T-  •  sm(0)  •  sm(0)—  +  —  •  y  •  cos(0)  sm(0)  — 
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At 

Af 
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Similarly,  the  system  noise  covariance  values,  crxl^,  crxj^,  and  crjc^^  are  also  derived  from 
the  target  covariances  using  Equation  5.30  through  5.33. 


II 

4^^ 

•  sin| 

[(xxl,) 

■  cos  (crjcJ ,) 

•  At 

(5.30) 

axl, 

=  o-xlt  ■ 

•  cos 

{(xxl) 

•  At 

(5.31) 

II 

4^^ 

•  sin| 

[(XXoj) 

•sin((rxy 

•  At 

(5.32) 

Equation  5.33  through  5.38  describe  the  the  state  transition  model  matrix.  A,  the 
control-input  model,  B,  the  control  vector,  u,  the  system  noise  covariance  matrix,  Q,  the 
measurement  model,  C,  and  the  measurement  noise  covariance  matrix,  R. 
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R  = 

0 

(^(yy) 

0 

0 

0 

1 

0 

'  0  0 

0 

0 

o-(yz) 

(5.36) 
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For  all  test  cases  within  Chapter  5,  EKF-A  used  the  target  initial  conditions  and 
accurate  control  law  based  on  the  target’s  initial  conditions.  PKF-B  already  demonstrated 
within  Chapter  3  the  inaccuracies  of  the  Kalman  filter  when  provided  with  inaccurate  initial 
conditions  or  inaccurate  control  laws.  Additionally,  the  measurements,  u,  v,  and  w  were 
rotated  using  Equation  4.19. 


5.3  Evaluated  Particle  Filter  A 

EPE-A  simulates  the  measurements  received  by  two  cameras,  u,  v,  and  w,  as  well 
camera  pan  and  tilt  angles.  Although  this  information  would  normally  be  obtained  from  two 
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cameras,  only  one  eamera  rotation  is  used  in  order  to  simplify  the  modeling  ealeulations 
and  reduee  potential  unintended  sourees  of  error.  Three  types  of  seenarios  were  tested: 

1 .  Movement  about  the  orthogonal  axes  with  eonstant  veloeity 

2.  Movement  in  a  eirele  about  the  three  prineipal  axes  with  eonstant  speed 

3.  Movement  with  eonstant  aeeeleration  for  veloeity  and  heading  angles 

Although  the  seenarios  use  different  initial  eonditions,  several  of  the  parameters  are 
eonstant,  as  defined  in  Table  5.1.  A  time  step  of  0.1  was  ehosen  for  the  purposes  of  sealing. 


Table  5.1:  EPF-A  Seenario  Parameters 


Parameter 

EPF-A 

Number  of  Simulations 

100 

Time  Step 

0.1 

Number  of  Partieles 

500 

5.3.1  Orthogonal  Axes  Movement. 

Six  seenarios  were  eondueted,  two  about  eaeh  axis,  one  with  no  measurement  noise 
and  one  with  measurement  noise.  All  simulations  ran  for  150  time  steps.  Table  5.2  lists 
the  initial  eonditions  for  eaeh  seenario  and  Table  5.3  lists  the  the  varianee  when  noise  was 
used. 
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Table  5.2:  Orthogonal  Axes  Initial  Conditions 


States 

X 

z 

V 

0 

(p 

Av: 

Ay 

Az 

AV 

^e 

A0 

x-Axis  Movement 

Target 

1 

0 

0 

4 

90 

0 

0 

0 

0 

0 

0 

0 

SLMA 

1 

0 

0 

4 

90 

0 

0 

0 

0 

0 

0 

0 

EPF-A 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

y-- 

Axis  Movement 

Target 

0 

1 

0 

4 

0 

0 

0 

0 

0 

0 

0 

0 

SLMA 

0 

1 

0 

4 

0 

0 

0 

0 

0 

0 

0 

0 

EPF-A 

0 

1 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

z-Axis  Movement 

Target: 

0 

0 

10 

4 

90 

90 

0 

0 

0 

0 

0 

0 

SLMA 

0 

0 

10 

4 

90 

90 

0 

0 

0 

0 

0 

0 

EPF-A 

0 

0 

10 

0 

0 

0 

0 

0 

0 

0 

0 

0 

EPF-A  Variance: 

1 

1 

1 

1 

0.1 

0.1 

0.5 

0.5 

0.5 

0.1 

0.01 

0.01 

Table  5.3:  Measurement  Noise  Variations 


No  Noise  Noise 


Measurements 

u 

V 

w 

u 

V 

w 

Target 

0 

0 

0 

5 

5 

5 

EPF-A 

0 

0 

0 

5 

5 

5 
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The  performance  of  each  axial  movement  was  evaluated  using  the  performance 
metrics  discussed  in  Section  5.2.1.  Axial  movements  were  first  evaluated  without 
measurement  noise.  Figure  5.3  contains  the  MAE  values  without  noise  for  EPF-A  and 
SEMA.  Figure  5.4  contains  the  TER  values  without  noise  for  EPF-A  and  SEMA.  A  single 
run  for  movement  about  each  axis  is  shown  in  Figure  5.5,  5.6,  and  5.7.  The  abbreviations 
used  within  the  legend  of  each  plot  are  detailed  in  Table  5.4. 


Table  5.4:  Evaluated  Particle  Filter  A  Eegend  Acronyms 


Acronym 

Meaning 

P 

EPF-A 

E 

SEMA 

K 

EKF-A 

X 

Axial  movement  along  X  axis 

y 

Axial  movement  along  Y  axis 

z 

Axial  movement  along  Z  axis 

xy 

Circular  rotation  in  X/Y  plane 

xz 

Circular  rotation  in  X/Z  plane 

yz 

Circular  rotation  in  Y/Z  plane 
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Error  (Deg)  Error  (Deg)  Error 


Distance  Error 


Velocity  Error 


—  P:x  c  L:x  -  a-  -  K;x  — * —  Py  o  L.y  -  a  -  Ky  — ♦ —  P:z  o  L:z  -  a  K:z 


Figure  5.3:  Mean  Error  for  Axial  Movement  without  Measurement  Noise 
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Distance  Error 


^ —  P;x  c  L:x  -  A-  -  K;x  — * —  Py  o  L.y  -  a  -  Ky  — ♦ —  P:z  o  L:z  -  a  K:z 


Figure  5.4:  Threshold  Error  for  Axial  Movement  without  Measurement  Noise 
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Position  Position 


X  Position 


Figure  5.5:  Single  Simulation  for  x-Axis  Movement  without  Measurement  Noise 
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Position  ^  Position 

Position 


X  Position 


Figure  5.6:  Single  Simulation  for  y-Axis  Movement  without  Measurement  Noise 


85 


Position 


X  Position 


Figure  5.7:  Single  Simulation  for  z-Axis  Movement  without  Measurement  Noise 
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Figure  5.3  and  5.4  demonstrate  the  earlier  concerns  of  the  MAE  discussed  in 
Section  5. 2. 1.1,  that  is  that  the  MAE  may  obscure  the  filter’s  actual  performance. 
Additionally,  Eigure  5.5,  5.6,  and  5.7  show  the  ossilatory  nature  of  EPE-A  based  on  its 
model,  discussed  in  Section  5.3.  EPE-A’s  model  could  be  optimized  for  a  linear  model, 
though  for  that  the  Kalman  filter  is  more  suitable.  Table  5.5  details  both  the  overall  mean 
TER  and  MAE  values  for  each  variable  for  the  last  50  time  units.  The  last  50  were  used 
since  by  this  point,  the  EPE-A  was  tracking  the  target  and  no  longer  searching  as  it  was 
within  approximately  the  first  25  time  units. 
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Table  5.5:  Mean  Axial  Errors  for  Last  50  Time  Steps  without  Measurement  Noise 


Metrics 

D 

V 

e 

0 

Mean  Error 

SLMA  X-Axis 

0.0000 

0.0000 

0.0000 

0.0000 

EKE-A  X-Axis 

0.0000 

3.6000 

0.0000 

0.0000 

EPE-A  X-Axis 

0.0845 

0.1970 

3.1749 

3.2342 

SLMA  Y-Axis 

0.0000 

0.0000 

0.0000 

84.7059 

EKE-A  Y-Axis 

0.0000 

3.6000 

0.0000 

0.0000 

EPE-A  Y-Axis 

0.0708 

0.2322 

21.1460 

90.6877 

SLMA  Z-Axis 

0.0000 

0.0000 

0.0000 

0.0000 

EKE-A  Z-Axis 

0.0000 

3.6000 

0.0000 

0.0000 

EPE-A  Z-Axis 

0.0837 

0.2063 

3.2913 

3.2316 

Threshold  Error 


SLMA  X-Axis 

0.0000 

0.0000 

0.0000 

0.0000 

EKE-A  X-Axis 

0.0000 

3.6000 

0.0000 

0.0000 

EPE-A  X-Axis 

0.0534 

0.3775 

6.6001 

6.4208 

SLMA  Y-Axis 

0.0000 

0.0000 

0.0000 

130.5882 

EKE-A  Y-Axis 

0.0000 

3.6000 

0.0000 

0.0000 

EPE-A  Y-Axis 

0.1319 

0.4792 

23.1609 

158.6222 

SLMA  Z-Axis 

0.0000 

0.0000 

0.0000 

0.0000 

EKE-A  Z-Axis 

0.0000 

3.6000 

0.0000 

0.0000 

EPE-A  Z-Axis 

0.1455 

0.4140 

6.6470 

6.6012 
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Overall  the  TER  is  greater  than  MAE,  indicating  that  there  are  not  many  distant 
outliers.  A  common  trend  was  that  the  TER  was  roughly  twice  the  MAE.  Additionally, 
since  no  noise  is  present  and  the  movement  is  completely  linear  (straight  line,  constant 
velocity),  the  SEMA  performs  better  than  EPE-A.  Eurthermore,  due  to  the  stochastic  nature 
of  the  filter  as  discussed  in  Section  5.2,  EPE-A  has  a  near  zero  likelihood  of  having  zero 
error.  In  order  to  further  verify  that  the  distribution  is  regular,  the  TER  for  several  different 
percentile  values  was  calculated  and  plotted  in  Eigure  5.8. 
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Distance  Error 


Theta  Heading  Error 


Figure  5.8:  Comparison  of  Threshold  Values  for  X-Axis  Movement  with  No  Measurement 
Noise 


90 


As  seen  in  Figure  5.8,  the  TERs  showed  regularly  increasing  error  with  increasing 
percentile.  Since  they  did  not  all  tend  towards  a  common  nonzero  value,  this  indicated 
there  is  little  chance  of  bias  being  present.  Alternatively,  bias  did  exist  within  the  first 
25  time  steps  when  the  filter  was  tracking  the  target.  This  bias  existed  due  to  incorrect 
initial  conditions.  Also  of  note  is  the  increase  in  6  heading  error  present  in  EPF-A  for 
movement  along  the  y-axis.  In  theory,  this  error  should  cause  an  increase  in  position  error 
since  the  direction  for  speed  is  not  in  the  direction  of  the  target.  Eor  this  scenario,  the 
target  9  should  be  equal  to  90,  which  is  equal  to  the  target  value.  However,  the  cosine  of 
18.7662  and  16.7740  are  0.9468  and  0.9613  respectively.  When  the  variance  for  9,  0.1,  is 
added  during  the  importance  sampling  step,  particles  that  match  the  target  9  are  generated, 
thus  allowing  the  particle  filter  to  continue  tracking  the  target  despite  the  heading  error. 
The  errors  in  (p  are  due  to  the  presence  of  singularity  about  the  y-axis.  The  potential  for 
less  accurate  particles  to  be  chosen  increases,  since  one  of  the  measurements  and  hence 
selection  criteria,  (f>,  is  meaningless  and  cannot  be  used  as  a  discriminator.  This  is  not  a  flaw 
on  the  particle  filter,  but  rather  a  flaw  inherent  in  the  parametrization  of  the  variables.  In 
order  to  avoid  singularities,  and  increase  accuracy,  an  alternate  method  of  parametrization, 
such  as  quaternions,  should  be  used.  Subsequent  scenarios  introduced  measurement  noise 
as  seen  in  Figure  5.9  and  5.10.  Figure  5.9  contains  the  MAE  values  with  noise  for  EPF-A 
and  SEMA  and  Figure  5.10  contains  the  TER  with  noise.  A  single  run  for  movement  about 
each  axis  is  shown  in  Figure  5.11,  5.12,  and  5.13. 
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Error  (Deg)  Error  (Deg)  Error 


Distance  Error 


Velocity  Error 


^ —  P;x  c  L:x  -  A-  -  K;x  — * —  Py  o  L.y  -  a  -  Ky  — ♦ —  P:z  o  L:z  -  a  K:z 


Figure  5.9:  Mean  Error  for  Axial  Movement  with  Measurement  Noise 
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Distance  Error 


^ —  P;x  c  L:x  -  A-  -  K;x  — * —  Py  o  L.y  -  a  -  Ky  — ♦ —  P:z  o  L:z  -  a  K:z 


Figure  5.10:  Threshold  Error  for  Axial  Movement  with  Measurement  Noise 
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Position  Position  Position 


X  Position 


Figure  5.1 1:  Single  Simulation  for  x-Axis  Movement  with  Measurement  Noise 
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Position  Position  Position 


X  Position 


Figure  5.12:  Single  Simulation  for  y-Axis  Movement  with  Measurement  Noise 
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Position  Position  Position 


X  Position 


Figure  5.13:  Single  Simulation  for  z-Axis  Movement  with  Measurement  Noise 
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With  the  addition  of  measurement  noise,  the  SLMA  performs  significantly  worse  than 
the  EPF- A.  Table  5.6  details  both  the  overall  mean  TER  and  MAE  values  for  each  variable 
for  the  last  50  time  units.  The  last  50  were  used  since  by  this  point,  the  EPF-A  was  tracking 
the  target  and  no  longer  searching  as  it  was  within  approximately  the  first  25  time  units. 
The  EKE- A  still  exceeds  the  performance  of  EPF-A  though  this  is  not  unexpected  given  the 
linear  movement  of  the  target.  Of  note  is  the  noticeable  increase  in  (f>  heading  for  movement 
in  the  y  direction.  The  cause  of  this  is  linked  to  the  singularity  around  the  y-axis  for  6. 
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Table  5.6:  Mean  Axial  Errors  for  Last  50  Time  Steps  with  Measurement  Noise 


Metrics 

D 

V 

e 

0 

Mean  Error 

SLMA  X-Axis 

3.9480 

127.9528 

32.5460 

87.5482 

PKE-A  X-Axis 

0.6449 

3.6000 

0.0000 

0.0000 

EPE-A  X-Axis 

0.9270 

1.1662 

16.2461 

18.0057 

SLMA  Y-Axis 

4.0027 

129.8334 

88.3252 

90.0457 

PKE-A  Y-Axis 

0.6449 

3.6000 

0.0000 

0.0000 

EPE-A  Y-Axis 

0.8610 

0.9681 

39.2148 

89.4936 

SLMA  Z-Axis 

3.9668 

128.4708 

32.3721 

88.2372 

PKE-A  Z-Axis 

0.6449 

3.6000 

0.0000 

0.0000 

EPE-A  Z-Axis 

0.9425 

1.1267 

15.9382 

18.3654 

Threshold  Error 


SLMA  X-Axis 

8.0090 

199.9310 

62.6127 

160.7167 

PKE-A  X-Axis 

0.6449 

3.6000 

0.0000 

0.0000 

EPE-A  X-Axis 

1.8840 

2.3915 

32.8683 

36.5628 

SLMA  Y-Axis 

8.1987 

204.2919 

139.8049 

159.6471 

PKE-A  Y-Axis 

0.6449 

3.6000 

0.0000 

0.0000 

EPE-A  Y-Axis 

1.7465 

1.9768 

50.0713 

160.0725 

SLMA  Z-Axis 

8.0351 

200.2442 

62.6789 

158.6492 

PKE-A  Z-Axis 

0.6449 

3.6000 

0.0000 

0.0000 

EPE-A  Z-Axis 

1.8864 

2.2948 

32.2677 

37.7447 
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5.3.2  Circular  Movement. 


Six  scenarios  were  conducted:  two  about  each  rotation  axis  where  one  had  perfect 
measurements  and  the  other  had  measurement  noise.  All  scenarios  ran  for  360  iterations, 
so  as  to  complete  one  full  circular  revolution.  Table  5.7  lists  the  initial  conditions  for  each 
scenario.  Measurement  noise  covariances  were  the  same  as  those  used  in  the  orthogonal 
axis  scenarios,  see  Table  5.3. 
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Table  5.7:  Circular  Rotation  Initial  Conditions 


States 

X 

z 

V 

0 

(p 

Ax 

Az 

AV 

^e 

A0 

Z-Axis  Circular  Rotation  in  the  XY-Plane 

Target 

0 

0 

20 

5 

90 

0 

0 

0 

0 

0 

1 

0 

SEMA 

0 

0 

20 

5 

90 

0 

0 

0 

0 

0 

0 

0 

EPF-A 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

Y-Axis  Circular  Rotation  in  the  XZ-Plane 

Target 

0 

0 

10 

5 

90 

0 

0 

0 

0 

0 

0 

1 

SEMA 

0 

0 

10 

5 

90 

0 

0 

0 

0 

0 

0 

0 

EPF-A 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

X-Axis  Circular  Rotation  in  the  YZ-Plane 

Target: 

0 

0 

10 

5 

0 

90 

0 

0 

0 

1 

0 

0 

SEMA 

0 

0 

10 

5 

9 

90 

0 

0 

0 

0 

0 

0 

EPF-A 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

EPF-A  Variance: 

1 

1 

1 

1 

0.1 

0.1 

0.5 

0.5 

0.5 

0.1 

0.01 

0.01 

The  performance  of  each  circular  rotation  was  evaluated  using  the  performance 
metrics  discussed  in  Section  5.2.1.  Circular  rotations  were  first  evaluated  without 
measurement  noise.  Figure  5.14  contains  the  MAE  values  without  noise  for  EPF-A  and 
SEMA  and  Figure  5.15  contains  the  TER  without  noise.  A  single  run  for  movement  about 
each  axis  is  shown  in  Figure  5.16,  5.17,  and  5.18. 
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Error  (Deg)  Error  (Deg) 


Distance  Error 


4 —  P:xy  G  L:xy  -  a-  K:xy  — ♦ —  P:xz  o  L:xz  -  a-  -  K:xz  — * —  Pyz  o  Lyz  -  a-  K:yz 


Figure  5.14:  Mean  Error  for  Cireular  Rotation  without  Measurement  Noise 
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Distance  Error 


-t —  P:xy  G  L:xy  -  a-  K:xy  — ♦ —  P:xz  o  L:xz  -  a-  -  K:xz  — * —  Pyz  o  Lyz  -  a-  K:yz 


Figure  5.15:  Threshold  Error  for  Circular  Rotation  without  Measurement  Noise 
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Position  Position  Position 


X  Position 


y  Position 


Figure  5.16:  Single  Simulation  for  xy-Plane  Cireular  Rotation  with  Measurement  Noise 
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X  Position 


Figure  5.17:  Single  Simulation  for  xz-Plane  Cireular  Rotation  with  Measurement  Noise 
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Position  Position  Position 


X  Position 


Figure  5.18:  Single  Simulation  for  yz-Plane  Cireular  Rotation  with  Measurement  Noise 
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Although  the  SLMA  performs  better  than  the  EPF-A  for  position  estimates  without 
noise,  its  estimation  of  the  velocity,  V,  and  headings,  6  and  (j),  are  noticeably  worse 
that  the  EPF-A.  This  is  due  to  the  sinusoidal,  and  thus  non-linear,  motion  of  the  target. 
Overall,  the  EPF-A  does  perform  better  than  the  SEMA  except  for  a  few  noticeable  sudden 
spikes  in  the  heading  angles.  Similarly  to  the  singularity  for  the  y-axis,  these  spikes 
are  due  to  parametrization.  The  spikes  occur  when  6  ox  (p  reach  the  end  of  their  range 
and  must  instantaneously  sweep  to  the  other  end  of  its  range.  Figure  5.19  illustrates  the 
relationship  between  the  target  and  EPF-A  variables  for  the  three  circular  scenarios  without 
measurement  noise.  EKE  had  comparable  performance  to  EPF-A  for  position  estimates, 
sometimes  worse,  sometimes  better  depending  on  the  metric  used.  However,  EKF-A 
encountered  significant  errors  when  estimating  the  hidden  states,  velocity  and  headings. 
The  cause  of  this  error  stems  from  the  type  of  Kalman  filter  EKF-A  is.  EKF-A  is  an 
optimal  gain  estimator  Kalman  filter;  this  means  that  it  will  optimize  its  Kalman  gains 
for  states  with  corresponding  measurements.  Since  velocity  and  headings  do  not  have 
measurements,  EKF-A  will  not  return  accurate  predictions.  Rather,  it  will  continue  to 
make  predictions  based  on  the  initial  conditions  and  control  law.  For  instances  in  which 
the  hidden  states  do  not  change,  such  as  the  axial  tests  (Section  5.3.1),  the  predictions  will 
be  accurate.  However,  as  soon  as  the  hidden  states  change  from  their  initial  conditions, 
EKF-A’s  predictions  will  begin  to  become  inaccurate  as  demonstrated  in  this  section. 
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Figure  5.19:  Target  and  EPF-A  Variable  Relationships 
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Target  Phi  (Deg)  Target  Theta  (Deg) 


In  order  to  generate  mean  values  of  the  TER  and  MAE,  variable  time  ranges  are 
used  depending  on  the  scenario  to  avoid  capturing  error  due  to  the  parametrization  and  not 
explicitly  the  filter  itself.  The  ranges  are  used  for  each  scenario  are  as  detailed  in  Table  5.8. 
Table  5.9  details  the  mean  values  of  TER  and  MAE  for  each  variable  using  variable  time 
ranges  of  50  time  units. 


Table  5.8:  Circular  Motion  Time  Unit  Ranges  for  Mean  Error 


Scenario 

Time  Range 

XY-Circle 

160-210 

XZ-Circle 

160-210 

YZ-Circle 

260-310 
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Table  5.9:  Mean  Circular  Errors  for  Variable  Time  Steps  without  Measurement  Noise 


Metrics 

D 

V 

e 

0 

Mean  Error 

SEMA  XY-Circle 

0.0043 

0.0006 

0.5000 

0.0000 

EKE-A  XY-Circle 

0.0000 

0.4264 

87.7572 

180.0000 

EPE-A  XY-Circle 

0.0388 

0.2188 

4.0210 

3.0164 

SEMA  XZ-Circle 

0.0050 

0.0006 

0.0000 

0.5000 

EKE-A  XZ-Circle 

0.0000 

0.4264 

0.0000 

100.2428 

EPE-A  XZ-Circle 

0.0353 

0.2289 

3.0135 

3.0276 

SEMA  YZ-Circle 

0.0027 

0.0007 

0.4991 

0.0000 

EKE-A  YZ-Circle 

0.0000 

2.1176 

14.8507 

180.0000 

EPE-A  YZ-Circle 

0.0749 

0.2180 

3.9874 

3.0637 

Threshold  Error 


SEMA  XY-Circle 

0.0043 

0.0006 

0.5000 

0.0000 

EKE-A  XY-Circle 

0.0000 

0.4264 

87.7572 

180.0000 

EPE-A  XY-Circle 

0.0792 

0.4439 

7.9134 

6.0172 

SEMA  XZ-Circle 

0.0050 

0.0006 

0.0000 

0.5000 

EKE-A  XZ-Circle 

0.0000 

0.4264 

0.0000 

100.2428 

EPE-A  XZ-Circle 

0.0717 

0.4700 

6.0365 

6.0905 

SEMA  YZ-Circle 

0.0027 

0.0007 

0.4991 

0.0000 

EKE-A  YZ-Circle 

0.0000 

2.1176 

0.0000 

160.9632 

EPE-A  YZ-Circle 

0.1353 

0.4381 

7.7922 

6.1927 
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Similarly  to  the  orthogonal  movements,  the  addition  of  noise  eontinues  to  degrade  the 
performanee  of  the  SLMA  while  minimally  affeeting  the  EKF-A  (for  observable  states) 
and  EPF-A  in  eomparison.  Figure  5.20  eontains  the  MAE  values  with  noise  for  EPF-A  and 
SFMA  and  Figure  5.21  eontains  the  TER  with  noise.  A  single  run  for  movement  about 
eaeh  axis  is  shown  in  Figure  5.22,  5.23,  and  5.24. 
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Figure  5.20:  Mean  Error  for  Circular  Rotation  with  Measurement  Noise 
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-t —  P:xy  G  L:xy  -  a-  K:xy  — ♦ —  P:xz  o  L:xz  -  a-  -  K:xz  — * —  Pyz  o  Lyz  -  a-  K:yz 


Figure  5.21:  Threshold  Error  for  Cireular  Rotation  with  Measurement  Noise 
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Position  Position  Position 
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Figure  5.22:  Single  Simulation  for  xy-Plane  Cireular  Rotation  with  Measurement  Noise 
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Position  Position  Position 


X  Position 


Figure  5.23:  Single  Simulation  for  xz-Plane  Cireular  Rotation  with  Measurement  Noise 
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Position  Position  Position 


X  Position 


Figure  5.24:  Single  Simulation  for  yz-Plane  Cireular  Rotation  with  Measurement  Noise 
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The  addition  of  noise  exaeerbates  the  inaeeuraeies  due  to  the  parametrization. 
Additionally,  the  90  pereentile  is  nearly  eonsistently  twiee  the  TER,  showing  that  the  shape 
of  the  distribution  in  error  ehanged  little  between  the  tests.  Table  5.10  details  both  the 
overall  mean  TER  and  MAE  values  for  eaeh  variable  using  variable  time  ranges  of  50  time 
units.  As  with  the  simulations  without  noise,  variable  time  ranges  are  used  depending  on 
the  seenario  to  avoid  eapturing  error  due  to  the  parametrization  and  not  explieitly  the  filter 
itself.  The  ranges  used  for  eaeh  seenario  are  detailed  in  Table  5.8. 
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Table  5.10:  Mean  Circular  Errors  for  Variable  Time  Steps  with  Measurement  Noise 


Metrics 

D 

V 

e 

0 

SEMA  XY-Circle 

3.9840 

128.6077 

34.9708 

87.1440 

EKE-A  XY-Circle 

1.0912 

0.4264 

87.7572 

180.0000 

EPE-A  XY-Circle 

0.9117 

1.4485 

17.7539 

17.5333 

SEMA  XZ-Circle 

3.9557 

128.4116 

33.3388 

87.4081 

EKE-A  XZ-Circle 

1.1349 

0.4264 

0.0000 

100.2428 

EPE-A  XZ-Circle 

0.8526 

1.4320 

16.6593 

17.3625 

SEMA  YZ-Circle 

4.0371 

129.6083 

35.9210 

87.7064 

EKE-A  YZ-Circle 

1.7615 

2.1176 

14.8507 

180.0000 

EPE-A  YZ-Circle 

1.0621 

1.3801 

18.2654 

17.7514 

Threshold  Error 


SEMA  XY-Circle 

8.1087 

200.1549 

67.8433 

158.9436 

EKE-A  XY-Circle 

2.1392 

0.4264 

87.7572 

180.0000 

EPE-A  XY-Circle 

1.8668 

2.9561 

35.8416 

36.4283 

SEMA  XZ-Circle 

8.0401 

201.2325 

64.2590 

159.4563 

EKE-A  XZ-Circle 

2.2251 

0.4264 

0.0000 

100.2428 

EPE-A  XZ-Circle 

1.7261 

2.9432 

33.6400 

35.1490 

SEMA  YZ-Circle 

8.2161 

203.0007 

70.5211 

159.1183 

EKE-A  YZ-Circle 

3.0645 

2.1176 

0.0000 

160.9632 

EPE-A  YZ-Circle 

2.0837 

2.8536 

36.7118 

36.2109 
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5.3.3  Unconstrained  Motion. 


Two  scenarios  were  conducted,  one  without  measurement  noise  and  one  with,  using 
a  set  of  initial  conditions  including  constant  acceleration,  detailed  in  Table  5.11.  Table  5.3 
details  the  noise  measurement  covariances  used.  The  states  were  unconstrained,  meaning 
the  target  was  not  restricted  in  its  movement  to  a  special  case,  as  was  done  in  Section  5.3.1 
and  5.3.2. 


Table  5.11:  Unconstrained  Motion  Initial  Conditions 


States 

X 

y 

z 

V 

e 

0 

h.x 

Ay 

Az 

AV 

A0 

A0 

Target 

5 

5 

5 

14 

45 

45 

0 

0 

0 

0.1 

5 

5 

SLMA 

1 

0 

0 

4 

90 

0 

0 

0 

0 

0 

0 

0 

EPF-A 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

EPE-A  Variance: 

1 

1 

1 

1 

0.1 

0.1 

0.5 

0.5 

0.5 

0.1 

0.01 

0.01 

The  angular  acceleration  variances  were  not  matched  in  order  to  reduce  the  variety 
of  the  heading  angles  during  the  re-sampling  step  of  the  particle  filter.  In  order  to  better 
visualize  the  motion  and  results.  Figure  5.25  contrasts  EPF-A’s  and  SLMA’s  performance 
against  the  target’s  actual  location  and  the  noisy,  measured  positions. 


118 


o 


Figure  5.25:  Single  Simulation  for  Uneonstrained  States,  in  Global  Frame,  with 
Measurement  Noise 


EPF-A  typieally  performed  better  than  SLMA,  as  expeeted.  The  exeeptions  are  when 
either  no  measurement  noise  was  present,  or  for  brief  periods  for  the  headings  6  and 
(f).  This  is  likely  due  the  faet  that  EPF-A  eneountered  diffieulties  traeking  due  to  the 
previously  mentioned  parametrization  flaws.  Additionally,  for  the  (p  heading  error,  the 
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SLMA  routinely  provided  a  severely  erroneous  heading,  as  seen  in  the  TER.  Despite  the 
threshold  error  remaining  near  1 80,  the  (f>  heading  does  not  have  a  eonstant  bias  sinee  its 
MAE  remained  near  90.  This  shows  that  the  distribution  was  near  uniform.  TER  and  MAE 
for  both  the  EPE-A  and  SEMA  are  seen  in  Eigure  5.27  and  5.26  respeetively.  Table  5.13 
details  the  overall  TER  and  MAE  errors  for  the  EPE-A  and  the  SEMA  for  the  last  50  time 
units. 
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Figure  5.26:  Mean  Error  for  Uneonstrained  Motion 
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Figure  5.27:  Threshold  Error  for  Uneonstrained  Motion 
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Table  5.12:  Mean  Unconstrained  Motion  Errors  for  Last  50  Time  Steps 


Metrics 

D 

V 

e 

(p 

Mean  Error 

SLMA  No  Noise 

0.0778 

0.0729 

2.5226 

4.5982 

EKE-A  No  Noise 

0.0000 

9.5998 

61.4653 

47.6136 

EPE-A  No  Noise 

0.2877 

0.9549 

9.4888 

37.2105 

SLMA  With  Noise 

3.8808 

115.9356 

51.1688 

84.7623 

EKE-A  With  Noise 

1.0146 

1.2061 

61.7888 

47.7020 

EPE-A  With  Noise 

1.5379 

2.0230 

20.2282 

55.0981 

Threshold  Error 


SLMA  No  Noise 

0.0778 

0.0729 

2.5226 

4.5982 

EKE-A  No  Noise 

0.0000 

9.5998 

61.4653 

47.6136 

EPE-A  No  Noise 

0.5909 

1.8809 

14.7539 

53.2478 

SLMA  With  Noise 

8.0344 

191.7217 

97.3950 

157.5901 

EKE-A  With  Noise 

1.0146 

1.2061 

61.7888 

47.7020 

EPE-A  With  Noise 

3.1024 

3.9957 

32.9966 

84.7418 

A  further  distinction  between  EPE-A  and  EKE-A  is  observed  with  the  addition  of 
system  noise.  As  discussed  in  Section  3.4. 5. 3,  once  the  control  law  does  not  match  the 
target  accelerations,  Kalman  filter  performance  begins  to  degrade.  100  simulations  were 
performed  with  a  system  noise  of  0.1  applied  to  V,  6,  and  Eigure  5.28  shows  the  mean 
error  for  100  simulation  runs.  Since  each  simulation  run  differs  due  to  the  random  system 
noise,  only  MAE  is  calculated. 
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Figure  5.28:  MAE  for  Equal  Weight  Matriees  with  System  Noise 
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As  expected,  EKF-A  performs  worse  than  EPF-A  due  to  the  inaccuracies  of  the  control 
law  for  even  slight  amounts  of  system  noise.  This  further  emphasizes  the  particle  filter’s 
superiority  for  predicting  movement  of  non-liner  systems  when  compared  to  a  optimal  gain 
Kalman  filter. 


Table  5.13:  Mean  Unconstrained  Motion  Errors  with  System  Noise  for  East  50  Time  Steps 


Metrics 

D 

V 

e 

Mean  Error 

SEMA 

3.9797 

115.5915 

49.3769 

83.0516 

EKF-A 

3.7723 

12.9709 

103.6987 

162.5783 

EPF-A 

1.8712 

2.8205 

25.2042 

51.9195 

5.3.4  Evaluated  Particle  Filter  A  Summary. 

Overall,  these  scenarios  demonstrated  the  ability  of  the  EPF-A  to  track  a  target  moving 
with  nonlinear  motion,  provided  only  noisy  measurements  of  the  target’s  location.  There 
are  a  few  discrepancies  when  compared  to  the  Kalman  filter,  however  these  errors  can  be 
traced  to  the  parameterization  effects.  Additionally,  EPF-A  was  able  to  determine  hidden 
target  states,  velocity  magnitude  and  heading,  that  could  aid  the  control  of  a  PTZ  camera(s) 
to  follow  the  target  more  smoothly  and  accurately. 

5.4  Evaluated  Particle  Filter  B 

EPF-B  simulates  the  measurements  received  by  a  single  camera,  Up,  Vp,  s,  Up, 
and  Vp,  as  well  as  camera  pan  and  tilt  angles.  Due  to  the  instability  of  this  filter, 
caused  by  its  frequent  propensity  to  suffer  from  weight  collapse,  a  single  set  of  initial 
conditions  were  used,  see  Table  5. 14.  The  weight  collapse  mitigation  techniques,  discussed 
in  in  Section  4.5,  were  evaluated  against  these  initial  conditions.  The  three  weight 
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mitigation  techniques  used  in  Section  5.4  are  Non-Compensated  Evaluation  Particle  Filter 
B  (NC),  Depth-Compensated  Evaluation  Particle  Filter  B  (DC),  and  Jacobian-Compensated 
Evaluation  Particle  Filter  B  (JC).  These  techniques  were  tested  both  with  and  without  noise. 
These  techniques  were  also  tested  using  two  different  weighing  matrices  in  an  attempt  to 
increase  depth  accuracy,  as  discussed  in  Section  4.5.3.  Additionally,  the  performance  of 
these  techniques  was  contrasted  against  the  SEMB,  as  well  as  EPF-A  and  SEMA.  The 
variables  used  by  EPF-A  and  SEMA  are  the  same  as  those  used  in  Section  5.3.  Since 
the  same  target  model  is  used  for  both  EPF-A  and  EPF-B,  the  measurements  used  by 
EPF-A  and  SEMA  are  extracted  before  the  target  model  generates  the  measurements  used 
by  EPF-B.  The  two  scenarios  tested  were: 

1 .  Equally  Weighted  Weighing  Matrix  (EW) 

2.  Unequally  Weighted  Weighing  Matrix  (UW) 

The  values  for  these  matrices  are  detailed  in  Equation  5.39. 


Q(Up) 

0 

0 

0 

0 

1 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

D(vp) 

0 

0 

0 

0 

1 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

D(5) 

0 

0 

0 

0 

1 

0 

0 

0 

0 

10 

0 

0 

0 

0 

0 

Q.(up) 

0 

0 

0 

0 

1 

0 

0 

0 

0 

10 

0 

0 

0 

0 

0 

Q.(Vp) 

0 

0 

0 

0 

1 

0 

0 

0 

0 

10 

Weighing  Matrix  Equal  Matrix  Unequal  Matrix 


The  initial  conditions  of  both  scenarios  are  shown  in  Table  5.14. 
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Table  5.14:  EPF-B  Scenario  Initial  Conditions 


States 

X 

y 

z 

V 

0 

0 

Ax 

Ay 

Az 

AV 

A0 

A0 

Target 

10 

10 

10 

5 

10 

10 

0 

0 

0 

0.2 

0.5 

0.5 

SEMA 

10 

10 

10 

5 

10 

10 

0 

0 

0 

0.2 

0.5 

0.5 

EPF-A 

10 

10 

10 

4 

10 

10 

0 

0 

0 

0 

0 

0 

SEMB 

10 

10 

10 

5 

10 

10 

0 

0 

0 

0.2 

0.5 

0.5 

EPF-B 

10 

10 

10 

4 

10 

10 

0 

0 

0 

0 

0 

0 

Filter  Variances 

EPF-A 

0.1 

0.1 

0.1 

0.5 

0.5 

0.5 

0.1 

0.1 

0.1 

0.1 

0.1 

0.1 

EPF-B 

0.1 

0.1 

0.1 

0.5 

0.5 

0.5 

0.1 

0.1 

0.1 

0.1 

0.1 

0.1 

The  two  scenarios  also  shared  several  parameters,  seen  in  Table  5.15.  The  same 
parameters  were  used  for  each  filter  in-order  to  allow  for  direct  comparisons.  A  step  size 
of  0.1  was  chosen  for  the  purposes  of  scaling. 


Table  5.15:  EPF-B  Scenario  Parameters 


Parameter 

EPF-A 

EPF-B 

Number  of  Simulations 

50 

50 

Number  of  Iterations 

50 

50 

Time  Step 

0.1 

0.1 

Number  of  Particles 

500 

1500 

The  measurement  noises  variances  used  by  both  scenarios  are  detailed  in  Table  5.16. 
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Table  5.16:  Measurement  Noise  Variations 


No  Noise  Noise 


Measurements 

u 

V 

w 

u 

V 

w 

Target 

0 

0 

0 

5 

5 

5 

EPF-A 

0 

0 

0 

5 

5 

5 

EPF-B 

0 

0 

0 

5 

5 

5 

5.4.1  Equal  Matrix. 

As  discussed,  all  three  weight  collapse  variations  were  tested  as  well  as  the  SLMB, 
EPF-A,  and  SLMB  to  provide  comparison.  The  TER  threshold  was  0.9.  A  single  set  of 
initial  conditions  was  used  for  two  scenarios,  without  and  with  noise.  All  results  for  both 
scenarios  are  shown  in  Table  5.20  to  provide  direct  comparison.  The  MAE  and  TER  for  this 
specific  scenario  are  shown  in  Figure  5.29  and  5.30  respectively.  The  abbreviations  used 
within  the  legend  of  each  plot  are  detailed  in  the  list  of  acronyms,  as  well  as  Table  5.17. 


Table  5.17:  Measurement  Noise  Variations 


Acronym 

Meaning 

NC 

Non-Compensated  EPF-B 

DC 

Depth-Compensated  EPF-B 

JC 

Jacobian-Compensated  EPF-B 

EW 

Equally  Weighted  Weighing  Matrix 

UW 

Unequally  Weighted  Weighing  Matrix 
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Figure  5.29:  MAE  for  Equal  Weight  Matrices  with  No  Noise 
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Error  (Deg)  Error  (Deg)  Error 
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Figure  5.30:  TER  for  Equal  Weight  Matriees  with  No  Noise 
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As  seen  in  both  Figure  5.29  and  5.30,  the  performance  of  the  three  mitigation  methods 
varied.  All  three  began  to  diverge  in  both  distance  and  velocity,  due  to  the  EPF-B’s  inability 
to  continue  tracking  a  target  as  the  velocity  continues  to  increase.  The  un-mitigated  EPF-B 
and  DC  had  little  discernible  difference;  indeed,  for  the  TER  the  DC  performed  worse  than 
the  uncompensated  model.  As  for  the  JC,  it’s  performance  was  less  degraded  since  it  could 
change  its  variance  to  compensate  for  the  increases  in  velocity.  However,  this  increase  in 
variance  appears  to  have  an  adverse  affect  on  the  heading  angles.  As  expected,  the  SEMB 
returned  the  most  erroneous  distance  and  velocity  values  and  continued  to  worsen  as  time 
progressed.  However,  the  SEMB  heading  errors  decreased  as  the  model  progressed,  but 
since  they  decreased  in  a  linear  fashion,  this  decrease  may  be  coincidental  since  the  target 
is  non-linear  with  changing  velocity.  Even  a  linear  model  should  reflect  this  acceleration 
via  a  changing  slope.  Eigure  5.31  and  5.32  show  the  effects  of  noise  on  the  various  models. 
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Figure  5.31:  MAE  for  Equal  Weight  Matrices  with  Noise 
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Figure  5.32:  TER  for  Equal  Weight  Matriees  with  Noise 
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With  the  addition  of  noise,  all  three  variations  of  EPF-B  decreased  their  error 
compared  to  the  simulations  without  noise.  Though  seemingly  paradoxical,  the  noise 
allows  the  filter  to  select  from  a  larger  spread  of  particles.  This  appears  to  mitigate  the 
lagging  issue  caused  by  insufficient  variance  for  the  predicted  states.  Additionally,  the  one 
filter  that  could  change  its  variance,  the  JC  becomes  locked  on  incorrect  (p  heading  values. 
When  the  JC  determines  the  velocity  is  increasing,  it  assigns  additional  variance  to  the 
velocity  prediction  at  the  expense  of  the  heading  angles.  If  the  variance  needed  to  predict 
a  state  decreases  to  the  point  that  the  spread  is  insufficient  to  allow  the  particle  to  move 
towards  the  observed  state,  then  a  lock  may  occur.  This  does  not  lead  to  weight  collapse 
however,  since  the  filter  can  still  use  measurements  for  other  variables. 

5.4.2  Unequal  Matrix. 

The  use  of  an  unequal  matrix  attempts  to  address  the  difficulties  in  determining  depth. 
By  increasing  the  weights  assigned  to  the  three  velocities,  5,  Up,  and  Vp,  it  may  be  possible  to 
more  accurately  determine  depth  if  the  filter  prefers  accurate  measurements  for  these  states. 
The  relationship  between  the  velocities  and  depth  is  seen  in  Equation  4.23.  Figure  5.33  and 
5.34  show  the  effects  of  the  unequal  weighing  matrix  without  measurement  noise. 
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Distance  Error 


Figure  5.33:  MAE  for  Unequal  Weight  Matrices  without  Noise 
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Error  (Deg)  Error  (Deg)  Error  Error 


Distance  Error 


Velocity  Error 


Theta  Heading  Error 


Figure  5.34:  TER  for  Unequal  Weight  Matrices  without  Noise 


136 


The  effects  of  the  the  unequal  weighted  matrix  are  limited  for  non-noisy  measure¬ 
ments.  There  is  slight  improvement  for  velocity,  and  consequently  distance,  errors  for  DC. 
There  was  little  to  no  improvement  for  the  un-compensated  or  JC  filters.  Effects  of  the 
unequal  weighted  matrix  with  measurement  noise  are  shown  in  Figure  5.35  and  5.36. 
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Theta  Heading  Error 


Figure  5.35:  MAE  for  Unequal  Weight  Matriees  with  Noise 
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Distance  Error 


Figure  5.36:  TER  for  Unequal  Weight  Matrices  with  Noise 
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The  addition  of  noise  had  limited  effeets  on  the  EPF-B  eompensator  variations.  Again, 
there  was  little  diseernible  differenee  between  the  equal  weighted  and  unequal  weighted 
results  for  the  uneompensated  or  JC  filters.  Figure  5.37  illustrates  that  there  does  not 
appear  to  be  any  diseernible  depth  improvement  between  equally  and  unequally  weighing 
matriees.  In  eontrast  to  previous  plots  of  Fuelidean  distanee,  Figure  5.33,  5.34,  5.35, 
and  5.36,  Figure  5.37  only  shows  the  depth.  Any  improvement  in  the  Euelidian  distanee 
is  due  to  better  estimates  of  Up  and  Uy,  not  depth,  sinee  all  variants  of  EPF-B  returned 
nearly  equivalent  erroneous  depth  estimates.  Additionally,  there  does  not  appear  to  be 
any  signifieant  differenee  between  the  three  EPF-B  filters.  The  MAE  and  TER  values 
are  nearly  identieal  sinee  the  sueeessful  simulations  of  the  filter  varied  little  from  eaeh 
other.  Both  MAE  and  TER  deereased  for  EPF-B  with  the  addition  of  noise,  eompared  to 
simulations  of  EPF-B  without  noise.  As  diseussed  in  Seetion  5.4.1,  the  differenee  between 
simulations  with  noise  and  those  without  is  not  due  to  better  filter  performanee,  but  rather 
lower  filter  toleranee.  The  results  improved  sinee  EPF-B  would  only  aeeept  more  aeeurate 
partiele  distributions  else  EPF-B  would  suffer  weight  eollapse  and  erash  until  it  reeeived 
a  more  aeeurate  partiele  distribution.  A  eomparison  of  the  weight  eollapses  is  detailed  in 
Table  5.21. 
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Error  Error  Error  Error 


Depth  Mean  Error  without  Noise 


Depth  Threshold  Error  without  Noise 


Depth  Mean  Error  with  Noise 


Figure  5.37:  Mean  and  Threshold  Depth  Error 


141 


Table  5.19  and  5.20  detail  the  MAE  and  TER  means  for  the  variations  of  EPE-B 


as  well  as  EPE-A,  SEMB,  and  SEMA,  without  and  with  measurement  noise  respectively. 
Since  all  variants  of  EPE-B  begin  to  diverge  after  25  time-steps  while  EPE-A  and  the  linear 
filters  converge  after  25  time-steps,  different  ranges  were  used  to  calculate  the  mean  values 
as  shown  in  Table  5.18.  This  was  done  to  provide  a  more  accurate  comparison. 


Table  5.18:  EPE-B  Time  Unit  Ranges  for  Mean  Error 


Eilter 

Time  Range 

EPE-B  :NC 

0-25 

EPE-B:DC 

0-25 

EPE-B  :JC 

0-25 

EPE-A: 

25-50 

SEMB: 

25-50 

SEMA: 

25-50 

142 


Table  5.19:  EPF-B  and  Comparison  Mean  Errors  for  Measurements  without  Noise 


Metrics 

D 

V 

e 

(p 

Mean  Absolute  Error 

EPE-B:  EW,  NC 

4.2604 

2.2522 

26.4100 

4.4401 

EPE-B:  EW,  DC 

5.0248 

2.4407 

26.3926 

4.6636 

EPE-B:  EW,  JC 

1.8196 

0.5765 

26.4099 

2.4577 

EPE-B:  UW,  NC 

4.3671 

2.1822 

26.4194 

3.8628 

EPE-B:  UW,  DC 

4.2236 

2.0363 

26.4469 

3.6290 

EPE-B:  UW,  JC 

2.0992 

0.7304 

26.3935 

2.8901 

EPE-A: 

0.1466 

0.3619 

13.5314 

6.8414 

SEMB: 

47.0796 

12.2979 

4.8728 

6.9306 

SEMA: 

0.0214 

0.1972 

4.8728 

6.9310 

Threshold  Error 

EPE-B:  EW,  NC 

11.3292 

4.5755 

27.5466 

9.5354 

EPE-B:  EW,  DC 

13.2298 

5.3592 

27.5452 

9.7998 

EPE-B:  EW,  JC 

4.8822 

1.2041 

27.4622 

5.3966 

EPE-B:  UW,  NC 

10.5499 

4.3951 

27.5951 

7.8076 

EPE-B:  UW,  DC 

9.6324 

3.5654 

27.5577 

7.1292 

EPE-B:  UW,  JC 

4.9113 

1.2384 

27.3523 

6.1313 

EPE-A: 

0.3051 

0.7234 

17.8481 

14.1513 

SEMB: 

59.9637 

12.2979 

4.8728 

6.9306 

SEMA: 

0.0272 

0.1972 

4.8728 

6.9310 
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Table  5.20:  EPF-B  and  Comparison  Mean  Errors  for  Measurements  with  Noise 


Metrics 

D 

V 

e 

(p 

Mean  Absolute  Error 

EPE-B:  EW,  NC 

2.2233 

0.9609 

26.3606 

1.6955 

EPE-B:  EW,  DC 

2.5175 

1.1234 

26.3990 

2.1413 

EPE-B:  EW,  JC 

1.5520 

0.4610 

26.3718 

1.1535 

EPE-B:  UW,  NC 

2.3045 

0.9605 

26.3809 

1.8514 

EPE-B:  UW,  DC 

2.0916 

0.9660 

26.3437 

1.8992 

EPE-B :UW,  JC 

1.4299 

0.9968 

29.2395 

41.2272 

EPE-A: 

1.4976 

1.1013 

28.7480 

40.4263 

SEMB: 

46.9995 

10.9148 

64.3976 

92.0850 

SEMA: 

3.8670 

40.0793 

52.7010 

81.2926 

Threshold  Error 

EPE-B:  EW,  NC 

5.0777 

1.7264 

27.3585 

3.5118 

EPE-B:  EW,  DC 

6.0791 

2.3830 

27.5268 

4.8032 

EPE-B:  EW,  JC 

3.8818 

0.8895 

26.9046 

2.4923 

EPE-B:  UW,  NC 

5.6134 

2.0294 

27.3875 

3.9196 

EPE-B:  UW,  DC 

5.6078 

1.9531 

27.3141 

3.8214 

EPE-B:  UW,  JC 

3.5946 

1.9848 

42.3245 

90.2125 

EPE-A: 

3.6158 

2.1992 

41.6683 

93.1232 

SEMB: 

59.9568 

11.6336 

121.0205 

165.1795 

SEMA: 

10.9642 

40.0239 

49.7049 

87.6773 
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An  additional  metric  used  is  the  number  of  weight  collapses  that  occurred  for  each 
particle  filter  per  number  of  simulations.  During  each  scenario,  the  number  of  times  a 
filter  crashed  due  to  weight  collapse  was  recorded  and  the  simulation  was  restarted  at  time 
t  =  0.  This  process  was  repeated  until  the  filter  completed  all  simulations  in  the  scenario 
successfully.  This  total  number  of  crashes  was  divided  by  the  number  of  simulations  used 
in  that  scenario.  Thus,  the  metric  can  be  interpreted  as  the  number  of  weight  collapses  that 
will  occur,  on  average,  for  each  successful  simulation.  It  serves  as  a  measure  of  stability. 
Neither  of  the  linear  filters,  SLMA  and  SLMB,  use  particles  and  thus  do  not  have  a  weight 
collapse  metric. 


Table  5.21:  Weight  Collapse  for  Measurements  without  and  with  Noise 


Without  Noise 

With  Noise 

EPF-B:  Equal  Weight,  No  Comp 

16.08 

86.30 

EPF-B:  Equal  Weight,  Depth 

12.82 

67.02 

EPF-B:  Equal  Weight,  Jacobain 

158.82 

806.46 

EPF-B:  Unequal  Weight,  No  Comp 

12.56 

77.86 

EPF-B:  Unequal  Weight,  Depth 

24.32 

50.48 

EPF-B:  Unequal  Weight,  Jacobain 

122.86 

771.20 

EPF-A: 

0 

0 

As  seen  in  Table  5.21,  EPF-A  never  suffered  a  weight  collapse  for  any  of  its 
simulations.  The  variants  of  EPF-B  suffered  additional  weight  collapse  with  the 
introduction  of  noise,  which  is  logical  since  it  is  more  difficult  to  determine  the  hidden 
states  in  the  presence  of  noise.  However,  due  to  the  high  rate  of  collapse,  none  of  the 
versions  of  EPF-A  are  mature  enough  to  be  used  without  additional  development  to  stem 
the  weight  collapse.  The  JC  suffered  the  most  frequent  weight  collapse  which  indicates  it 
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is  the  most  sensitive  variant  of  EPF-B.  Additionally,  the  high  rate  of  weight  collapse  for  JC 
indicates  the  filter  is  not  determining  the  best  variance  values  to  use. 

5.4.3  Evaluated  Particle  Filter  B  Summary. 

Based  on  the  results  from  the  EPF-B  evaluation  scenarios,  additional  development  is 
required  to  make  the  filter  viable.  All  variants  diverged  to  some  degree,  however  the  JC  did 
so  the  least,  indicating  the  possibility  that  positive  tracking  could  be  achieved.  However, 
the  JC  is  also  the  least  stable  filter  by  a  significant  factor.  Further  investigation  is  required 
to  determine  the  most  appropriate  method  to  change  the  variance. 
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VI.  Conclusions  and  Future  Work 


6.1  Conclusion 

This  thesis  developed  and  evaluated  the  possibility  of  using  a  particle  filter  to  track 
non-linear  targets  with  both  3-D  position  data  and  pixel  data  from  a  single  camera  or 
observer  without  accurate  depth  information.  In  order  to  track  non-linear  targets,  the  The 
ability  of  the  particle  filter  to  track  non-linear  targets  depends  on  its  ability  to  determine 
hidden  states,  velocity  and  headings,  using  both  clean  as  well  as  noisy  state  variables 
and  measurements.  Additionally,  the  particle  filter  used  nonlinear  models  as  its  basis. 
Simulations  of  vision  based  systems  were  developed  to  evaluate  the  particle  filter  for 
simulated  measurements  from  a  system  with  accurate  depth  measurements  and  one  without. 
Two  distinct  particle  filters  were  developed,  for  each  respective  system,  EPF-A  and  EPF-B. 
In  conjunction  with  the  particle  filters,  two  linear  models  were  also  developed,  SEMA  and 
SEMB,  and  a  Kalamn  filter,  EKF-A,  to  serve  as  a  baseline  comparisons  for  the  particle 
filters’  performance. 

EPF-A’s  performance  was  evaluated  using  a  variety  of  tests.  Two  series  of  tests 
constrained  the  movement  of  the  target,  thus  using  a  degenerate  form  of  the  model  present 
in  EPF-A,  in  order  to  evaluate  the  filter’s  ability  to  track  a  target  that  behaved  according 
to  a  subset  of  the  model  used  in  EPF-A.  The  filter  tracked  the  target  accurately  for  all 
hidden  states  while  SEMA  could  only  track  the  position  states  that  directly  correlated  to 
the  measurements  of  the  target.  With  the  addition  of  noise,  EPF-A  could  still  track  the 
hidden  states,  albet  with  slightly  less  accuracy.  However,  SEMA  could  not  track  any 
of  the  states.  The  final  series  of  tests  did  not  constrain  the  target  model,  and  included 
constant  acceleration.  EPF-A  closely  tracked  the  target,  despite  the  presence  of  noise, 
while  the  SEMA  could  not  track  the  target,  including  its  position.  EKF-A  could  track  the 
target,  however  its  hidden  state  estimations  were  less  accurate  than  those  of  EPF-AThis  test 


147 


demonstrated  that  the  filter  is  well-suited  to  track  targets  with  noisy  measurements  when 
the  filter  uses  an  accurate  model  to  generate  its  predictions. 

EPF-B’s  performance  was  also  evaluated,  though  only  a  single  scenario  was  used 
due  to  EPF-B’s  propensity  towards  weight  collapse.  Additional  scenarios  were  deemed 
unnecessary  until  the  weight  collapse  concern  is  addressed.  A  variety  of  methods  were 
evaluated  to  determine  if  any  could  reduce  weight  collapse.  Although  some  improved 
EPF-B’s  performance  marginally,  none  could  successfully  mitigate  the  weight  collapse 
issue.  This  is  a  key  area  for  improvement,  as  illustrated  by  Table  5.21.  Additionally,  the 
sensitivity  of  the  filter  to  measurements  prevents  it  from  being  used  as  effectively  as  EPF-A 
to  find  the  target.  Rather,  EPF-B  must  start  with  the  same  initial  conditions  as  the  target  in 
order  to  track  it. 

6.2  Practical  Considerations 

The  particle  filter,  being  probabilistic,  has  several  key  distinguishing  features  from 
traditional  linear  filters  that  must  be  considered  by  the  user. 

1 .  Filter  Model  -  First  and  foremost,  the  filter  model  should  match  the  target  as  closely 
as  possible.  If  the  filter  model  begins  to  differ  from  the  target,  then  the  particles  it 
produces  begin  to  differ  from  the  measurements,  consequently  producing  erroneous 
predictions.  Additionally,  since  the  filter  does  not  require  a  linear  model,  no  effort 
should  be  made  to  simplify  the  model  unless  due  to  computational  concerns  (in  which 
case  the  particle  filter  may  not  be  the  most  suitable  filter  anyway).  Additionally,  the 
model  is  what  allows  the  filter  to  produce  reasonable  estimates  despite  the  presence 
of  noise.  As  noise  variances  increase,  the  filter  relies  more  on  its  own  predictions 
rather  than  the  measurements.  If  these  predictions  are  inaccurate  due  to  a  poor 
model,  then  the  filter  will  eventually  diverge  from  the  target  regardless  of  noise. 
Even  with  perfect  measurements,  if  the  model  is  inaccurate,  the  filter  may  not  be 
able  to  determine  the  state  variables  of  interest.  One  potential  method  of  resolving 
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these  model  inaccuracies  is  to  incorporate  a  machine  learning  phase  into  the  filter 
and  allow  the  filter  to  alter  its  own  models  based  on  the  measurements. 

2.  State  Variables  -  Regarding  the  state  variables,  only  the  highest  order  variables  should 
be  varied  using  their  corresponding  system  variance.  If  lower  order  or  measurement 
variables  are  varied,  the  particle  filter  may  select  particles  due  to  the  variance. 
While  these  particles  may  more  closely  match  the  measurements,  they  will  no  longer 
correspond  with  their  respective  higher  order  variables  due  to  the  additional  variance 
‘contaminating’  their  value. 

3.  System  Variances  -  Similarly  to  the  model,  system  variances  should  match  the 
target’s  system  variances  as  close  as  possible.  The  system  variances  help  define  the 
target  distribution  from  which  particles  are  sampled.  If  the  variances  are  too  large, 
then  the  particles  are  spread  further  from  the  most  likely  states  resulting  in  decreased 
fidelity.  Additionally,  large  variances  may  result  in  multiple  viable  solutions  with 
complex  models,  as  demonstrated  by  EPF-B.  When  the  system  variance  became 
too  large,  the  particle  filter  could  select  measurements  that  resulted  in  incorrect  state 
variables  since  multiple  combinations  of  the  state  variables  could  yield  the  same 
measurements.  If  the  variances  are  too  tight,  then  the  correct  solutions  may  lie 
outside  the  distribution  of  particles.  This  too  can  cause  the  error  between  the  target 
and  filter  states  to  grow  since  the  filter  does  not  have  the  ability  to  produce  states 
equal  to  the  target  states.  This  phenomenon  was  also  observed  with  EPF-B  when  the 
speed  began  to  drift  behind  the  target  speed,  causing  a  cascading  ripple  through  the 
other  lower  order  states,  such  as  position.  If  left  uncorrected,  this  can  lead  to  weight 
collapse  since  the  filter  will  eventually  be  unable  to  generate  a  prediction  that  is  close 
enough  to  the  measurements  to  generate  a  sufficient  weight. 
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4.  Dimensionality  -  Similarly  to  other  filters,  as  the  number  of  dimensions  increases, 
the  size  of  the  state  space  must  also  increase  to  accommodate  these  dimensions. 
The  likelihood  of  weight  collapse  increases  as  the  number  of  dimensions  increases, 
since  the  potential  number  of  solutions  may  vastly  exceed  the  number  of  particles. 
Thus,  the  correct  solution  may  be  ‘lost’  in  the  state-space.  Adding  more  particles  to 
model  this  state  space  is  also  a  temporary  solution;  eventually  the  number  of  particles 
needed  to  accurately  model  the  state  space  will  exceed  the  computational  capabilities 
at  hand.  Efforts  to  mitigate  this  vulnerability  are  still  ongoing  [5]. 

5.  Measurement  Noise  -  Analogous  to  Kalman  gain,  the  measurement  noise  determines 
the  degree  to  which  the  filter  trusts  the  measurements.  The  larger  this  value,  the  more 
the  filter  will  rely  on  the  model  predictions,  once  again  illustrating  the  necessity 
of  an  accurate  model.  The  smaller  this  value,  the  more  the  filter  will  trust  the 
measurements. 

6.  Weighing  Matrix  -  The  weighing  matrix  is  an  additional  means  to  control  what 
the  filter  relies  on  for  its  measurements.  The  different  weights  correspond  to  how 
much  the  filter  will  rely  on  each  measurement  to  generate  the  importance  weight. 
Depending  on  the  system,  some  measurements  may  be  inherently  less  reliable  than 
others,  so  their  impact  should  be  minimized,  but  not  entirely  eliminated  since  they 
do  characterize  the  state  variables  to  some  degree.  Also,  if  the  user  wishes  to  track 
a  particular  state  with  increased  accuracy,  the  weights  of  the  measurements  that 
correspond  to  that  state  could  be  increased. 

7.  Weight  Distribution  -  Although  a  normal  distribution  was  used  in  this  research  to 
generate  the  importance  weights,  any  type  of  distribution  function  may  be  used,  so 
long  as  the  distribution  mirrors  that  target’s  noise  distribution. 
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8.  Statistics  -  The  end  product  of  the  particle  filter  is  a  discrete  probability  distribution. 
A  variety  of  statistical  methods  may  be  used  to  generate  the  predicted  state  variables; 
a  mean  is  one  of  the  simplest,  but  more  advanced  methods  will  likely  return  more 
robust  predictions. 

6.3  Future  Work 

There  are  three  key  lines  of  future  work  concerning  this  research:  real-world 
implementation,  further  development,  and  space-environment  simulation.  Although  the 
EPF-A  has  been  developed  and  tested  within  an  simulated  environment,  it  must  be  tested 
on  an  actual  system  in  order  to  fully  validate  its  functionality  and  performance.  The  most 
likely  system  is  the  PTZ  camera  setup  developed  at  AFIT.  Note,  this  current  setup  provides 
the  depth  measurement  needed  by  EPF-A  by  using  two  PTZ  cameras  to  provide  stereo 
vision.  The  particle  filter  could  be  used  in  conjunction  with  the  current  system  to  provide 
more  accurate  global  target  locations.  Ideally,  the  results  from  the  particle  filter  would 
be  used  to  direct  the  cameras  to  their  next  position,  anticipating  the  movement  of  the 
target  in  order  to  provide  smoother  and  more  accurate  tracking.  One  of  the  key  benefits 
of  the  particle  filter  is  that  multiple  non-linear  target  models  could  be  used  depending  on 
the  target’s  dynamic  properties.  Currently,  the  particle  filter  researched  in  this  thesis  is  a 
relatively  simple  SIS.  As  demonstrated  in  Section  3.4. 5. 4  and  discussed  in  Section  2.5.6, 
weight  collapse  remains  an  potential  issue  for  EPF-A  and  a  definite  issue  for  EPF-B.  More 
advanced  filters  could  mitigate  such  collapses  by  both  identifying  potential  collapses  and 
redistributing  the  particles  to  stave  off  a  collapse  and  by  improving  the  tracking  ability  of 
the  filter.  Better  tracking  results  in  higher  particle  weights,  thus  again  reducing  the  risk  of 
weight  collapse. 

One  such  area  of  improvement  that  is  unexplored  is  more  advanced  statistical  methods 
to  both  better  model  the  discrete  posterior  PDF  produced  by  the  particle  filter  as  well  as 
subsequently  sampling  this  PDF  to  produce  the  set  of  final  states.  Improvements  could 


151 


also  be  made  to  the  variances;  e.g.  how  they  are  generated  and  how  they  are  updated  as 
the  particle  filter  tracks  the  target.  This  area  was  explored  with  the  various  compensators 
developed  for  EPF-B  and  although  the  results  were  not  definite,  additional  exploration 
should  be  conducted.  Although  JC  holds  the  most  potential  to  estimate  depth,  due  to  its 
ability  to  alter  the  filter’s  variances,  in  its  present  form  it  is  unsuitable  due  to  its  severe 
weight  collapse  concerns. 

The  final  primary  line  of  research  focuses  on  using  the  particle  filter  to  track  space- 
objects  based  on  limited  measurements  and  subsequently  generate  orbit  properties  of  that 
object.  The  particle  filter  performs  ideally  when  the  model  used  by  the  filter  more  closely 
resembles  the  target.  The  space  environment  is  a  unique  environment,  one  that  has  been 
meticulously  modeled  and  where  models  are  able  to  predict  accurate  long-term  behavior 
of  objects.  The  particle  filter  could  be  used  to  observe  objects  and  determine  their  hidden 
states,  in  this  case  orbital  parameters,  potentially  using  a  small  set  of  measurements.  These 
measurements  could  be  vision-based  from  satellites,  and  possibly  taken  single  camera  if 
the  particle  filter  is  able  to  render  accurate  depth  information.  By  increasing  the  number 
of  sensors,  orbit  conflicts  can  be  mitigated,  helping  to  reduce  congestion  within  the  space 
environment. 
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Appendix:  MATLAB  Code 


A.l  Introduction 

The  following  appendices  contain  the  MATLAB  code  for  the  particle  filters  PPF-A, 
PPF-B,  EPF-A,  and  EPF-B,  as  well  as  the  Kalman  filters  and  linear  models,  PKE-A, 
PKE-B,  SEMA,  and  SEMB.  Also  included  are  the  functions  to  gnerate  the  plots  and 
metrics.  The  functions  are  grouped  based  on  which  particle  filter  is  being  evaluated. 

A.2  Prototype  Particle  Filter  A 

A  single  function,  PPP_A_Pinal.m,  executes  and  plots  PPE-A. 


clear  all;  close  all;  clc; 

%%  PPF-A_Final 

%This  program  will  execute  PPF— A  which  is  a  single  dimension  particle  fiter 
%using  a  highly  non-linear  function  to  demonstrate  the  particle  filter's 
%ability  to  track  non-linear  functions.  No  comparison  filter  is  provided 
%due  to  the  highly  non-linear  function. 

%%  Defaults 

%  Change  default  axes  fonts 

set ( 0 Def aultAxesFontName Times  New  Roman') 
set  ( 0 ,  ' Def aultAxesFontSize ' ,  14) 

%  Change  default  text  fonts 

set ( 0 , ' Def aultTextFontName ' ,  'Times  New  Roman') 
set  ( 0 ,  ' Def aultTextFontSize  '  ,  14) 

%%  Initialize  Variables 
%Initial  target  variables 

%Total  number  of  time  steps  (run  time) 
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T=7  0; 


t  =  l; 

%Initial  state 
x(l)=0.1; 

%Process /system  noise  covariance 
x_N=l; 

%Measurement  noise  covaraiance 
x_R=l; 

%Initial  observations 
%Target  measurement 

y(l)  =  [x(l)''2  /  20  +  sqrt  ( x_R)  *randn]  ; 

%Initial  particle  variables 
%Number  of  particles 
N=100; 

%Variance  of  initial  estimate 
V=2; 

%Initialize  particles  based  on  initial  conditions  (create  first 
%distribution  of  particles) 
for  i  =  1 : N 

x_P{l,i)  =  X  +  sqrt (V) *randn; 

end 


%%  Begin  simulating 
for  t  =  2 : T 
%%  Truth  Model 

%Propogate  state  forward 

x(t)=0.5*x(t— l)+25*x(t— 1)  /  (l+x(t— 1)  ''2)+8*cos  (1.2*  (t— 1)  )  +sqrt  ( x_N )  *randn; 
%Propogate  measurements  forward  based  on  states 
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y (t ) =x (t ) ^ 2 /2 0  +  sqrt (x_R) *rand; 


%%  Particle  Filter 
for  1=1 : N 

%Update  states  for  model 

x_P  (t,i)  =  .5*x_P  (t-l,i)+25*x_P  (t-l,i)  /  (l  +  x_P  (t-l,i)  ''2)  +  .  .  . 

8* cos (1.2* (t— 1) ) tsqrt (x_N) *randn; 

%Measurement  update 
y_P  (t,i)=x_P  (t,i)  ''2/20; 

%Generate  and  assign  importance  weights  to  particles 

P_w  (t,  i)  =  (l/sqrt  (2*pi*x_R)  )  *exp(-{y(t)-y_P  (t,  i)  )  "'2  /  (2*x_R)  )  ; 

end 

%Normalize  to  form  a  probability  distribution 
P_w{t,:)  =  P_w (t , : ) . /sum { P_w (t , : ) ) ; 

%Resample  particles  to  form  new  distribution 

%What  this  code  specifically  does  is  randomly,  uniformally,  sample  from 
%the  cummulative  distribution  of  the  probability  distribution 
%generated  by  the  weighted  vector  P_w.  If  you  sample  randomly  over 
%this  distribution,  you  will  select  values  based  upon  there  statistical 
%probability,  and  thus,  on  average,  pick  values  with  the  higher  weights 
%(i.e.  high  probability  of  being  correct  given  the  observation  z) . 
%store  this  new  value  to  the  new  estimate  which  will  go  back  into  the 
%next  iteration 
for  i=l : N 

P_get (t,  i) =find (rand<=cumsum (P_w (t,  : ) ) , 1 ) ; 
x_P  (t,i)=x_P  (t,P_get  {t,i)  )  ; 

end 

x_est  (t)  =  mean (x_P (t,  : ) ) ; 

end 
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t=l :T; 


hf ig=figure ( ' name ' , ' PPF— A' ) ; 
set(hfig,  'Position',  [100,100,  800,400]); 
plot (t,  X,  ' *-b ' ,  t,  x_est,  ' *-r ' ,  t,  y,  '* — g'); 
xlabel('Time  Step');  ylabel (' Position ') ; 
legend ( ' Target ' ,  ' PPF— A ' , ' Measurement ' ) ; 

set { legend,  ' Orientation '  ,  ' horizontal '  ,  'Location '  ,  ' S out hOut side  '  )  ; 


A.3  Prototype  Particle  Filter  B 

A  single  function,  PPF  B  JFinal.m,  executes  and  plots  PPF-B,  PKF-A,  and  PKF-B. 


clear  all;  close  all;  clc; 

%%  PPF-A_Final 

%This  program  will  execute  PPF— A  which  is  a  single  dimension  particle  fiter 
%using  a  highly  non-linear  function  to  demonstrate  the  particle  filter's 
%ability  to  track  non-linear  functions.  No  comparison  filter  is  provided 
%due  to  the  highly  non-linear  function. 

%%  Defaults 

%  Change  default  axes  fonts 

set ( 0 ,' Def aultAxesFontName ',' Times  New  Roman') 
set  ( 0 ,  ' Def aultAxesFontSize ' ,  14) 

%  Change  default  text  fonts 

set ( 0 , ' Def aultTextFontName ' ,  'Times  New  Roman') 
set  ( 0 ,  ' Def aultTextFontSize  '  ,  14) 

%%  Initialize  Variables 
%Initial  target  variables 

%Total  number  of  time  steps  (run  time) 

T=7  0; 
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t  =  l; 


%Initial  state 
x(l)=0.1; 

%Process /system  noise  covariance 
x_N=l ; 

%Measurement  noise  covaraiance 
x_R=l ; 

%Initial  observations 
%Target  measurement 

Y(1)  =  [x(l)''2  /  20  +  sqrt  ( x_R)  *randn]  ; 

%Initial  particle  variables 
%Number  of  particles 
N=100; 

%Variance  of  initial  estimate 
V=2; 

%Initialize  particles  based  on  initial  conditions  (create  first 
%distribution  of  particles) 
for  i  =  1 : N 

x_P(l,i)  =  X  +  sqrt (V) *randn; 

end 


%%  Begin  simulating 
for  t  =  2 : T 
%%  Truth  Model 

%Propogate  state  forward 

x(t)=0.5*x(t-l)+25*x(t-l)  /  (l+x(t-l)  ''2)+8*cos  (1.2*  (t-1)  )  +sqrt  ( x_N )  *randn; 
%Propogate  measurements  forward  based  on  states 
y (t ) =x (t ) " 2 /2 0  +  sqrt (x_R) *rand; 
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%%  Particle  Filter 
for  i=l : N 

%Update  states  for  model 

x_P  (t,i)  =  .5*x_P  (t-l,i)+25*x_P  (t-l,i)  /  (l  +  x_P  (t-l,i)  ''2)+.  .  . 

8* cos (1.2* (t-1) ) fsqrt ( x_N ) *randn ; 

%Measurement  update 
y_P  (t,i)=x_P  (t,i)  ''2/20; 

%Generate  and  assign  importance  weights  to  particles 

P_w  (t,  i)  =  (l/sqrt  (2*pi*x_R)  )  *exp(-{y(t)-y_P  (t,  i)  )  "'2  /  (2*x_R)  )  ; 

end 

%Normalize  to  form  a  probability  distribution 
P_w{t,:)  =  P_w (t , : ) . /sum { P_w (t , : ) ) ; 

%Resample  particles  to  form  new  distribution 

%What  this  code  specifically  does  is  randomly,  uniformally,  sample  from 
%the  cummulative  distribution  of  the  probability  distribution 
%generated  by  the  weighted  vector  P_w.  If  you  sample  randomly  over 
%this  distribution,  you  will  select  values  based  upon  there  statistical 
%probability ,  and  thus,  on  average,  pick  values  with  the  higher  weights 
%(i.e.  high  probability  of  being  correct  given  the  observation  z) . 
%store  this  new  value  to  the  new  estimate  which  will  go  back  into  the 
%next  iteration 
for  i=l : N 

P_get (t,  i) =find (rand<=cumsum (P_w (t,  :)),!); 
x_P  (t,i)=x_P  (t,P_get  {t,i)  )  ; 

end 

x_est  (t)  =  mean (x_P (t,  : ) ) ; 

end 
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t=l :T; 


hf ig=figure ( ' name ' , ' PPF— A' ) ; 
set(hfig,  'Position',  [100,100,  800,400]); 
plot (t,  X,  ' *-b ' ,  t,  x_est,  '*-r',  t,  y,  '* — g'); 
xlabel('Time  Step');  ylabel (' Position ') ; 
legend ( ' Target ' ,  ' PPF— A '  ,  ' Measurement ' )  ; 

set (legend,  ' Orientation '  ,  ' horizontal ' ,  ' Location ' ,  ' SouthOutside ' ) ; 


A.4  Evaluated  Particle  Filter  A 

Two  functions  executed  and  plotted  EPF-A,  EKF-A,  and  SEMA.  The  first 
funetion,  EPF  A  ExeeuteJFinal.m,  provides  the  initial  eonditions  to  the  seeond  funetion, 
EPF_A_Funetion_Final.m,  whieh  exeeutes  the  filters. 

A.4.1  EPF  Ji_ExecuteJ'inal. 


%%  This  script  analyzes  EPF— A 
clear  all;  close  all;  clc; 

%%  Defaults 

%  Change  default  axes  fonts 

set ( 0 ,' Def aultAxesFontName ',' Times  New  Roman') 
set  ( 0 ,  ' Def aultAxesFontSize ' ,  14) 


%  Change  default  text  fonts 

set ( 0 ,  ' Def aultTextFontName '  ,  'Times  New  Roman') 
set  ( 0 ,  ' Def aultTextFontSize  '  ,  14) 

%%  Parameters 

%Number  of  simulation  runs 
sim_run=l ; 
sim_plot=sim_run; 

%Threshold  Error  Range 
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solution_range  =  .9; 
%Iternation  Number 
params (1)  =  150; 

%Time  Steps 
params (2)  =  . 1 ; 

dt=params (2 ) ; 
T=params { 1 ) ; 


%%  Target  Parameters 


:Straight 

Line, 

X— axis : 

1 

:Straight 

Line, 

y-axis : 

2 

:Straight 

Line, 

z— axis : 

3 

:Straight 

Line, 

— X— 

axis  : 

;  4 

:Straight 

Line, 

-y- 

axis  : 

;  5 

:Straight 

Line, 

—  z— 

axis  : 

;  6 

iCircle, 

x/y,  about 

z  : 

7 

iCircle, 

x/z,  about 

y: 

8 

iCircle, 

y/z,  about 

X : 

9 

iPrelim,  chap  5  : 

10 

;  MANUAL, 

see  below: 

0 

target_scenario  =0; 

%Process  Variance 
%0 . 1 :  2 
%None :  0 

P_variance_scenario  =  2; 

1 
2 
3 
0 


%No  measurment  variance: 
%5  Measurement  variance: 
%10  Measurement  variance: 
%MANUAL : 

m_variance_scenario  =2; 
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Kalman  Filter  Parameters 


0  0 

%Initial  Conditions 
%Match  target  ICs :  1 

%Without,  at  0 :  2 

Kalman_ICs  =  1; 


%Control  Law 

%Axial  Simulation:  1 
%Circular  Simulation:  2 
%Random  Simulation:  3 


Kalman_Control_Law  =  1; 


%Process  Variance 
Kalman_Process_Variance  =  1; 


%Measurement  Variance 
Kalman_Measurement_Variance  =  1; 

%%  EPF— A  Parameters 
%With  target  intial  conditions:  1 
%Without,  at  0  (x,y,z) :  2 

%For  circle,  x/y  about  z:  3 

%For  circle,  x/z  about  y:  3 

%For  circle,  y/z  about  y:  3 

%MANUAL :  0 

f ilterl_scenario  =  2 ; 


%Number  of  particles 
p_num  =  500; 
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%Weights 
%A11  equal:  1 
%MANUAL :  0 

f ilterl_weight  =1; 

%Filter  variances 
%Scenario  1 :  1 

%Standard:  2 

%MANUAL :  0 

f ilterl_variance  =2; 

%Measurment  Variance 
%Match  measurment :  1 

%Variance  of  .01:  2 

%MANUAL :  0 

%N0TE:  CANNOT  BE  0 
f ilterl_measurement_variance  =  1; 

%Manual  values 


%Target 

manual { 1 ) 

5; 

%x 

manual  { 2 ) 

5; 

%y 

manual  { 3 ) 

5; 

%z 

manual { 4 ) 

4; 

%V 

manual  { 5 ) 

45; 

%theta 

manual { 6) 

45; 

%phi 

manual { 7 ) 

0; 

%dx 

manual  { 8 ) 

0; 

%dy 

manual { 9) 

0; 

%dz 

manual  (10) 

=  .1; 

%dV 

manual (11) 

=  5; 

%dtheta 

manual  { 12 ) 

=  5; 

%dphi 
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manual (13) 

=  0; 

%x_dot 

manual  { 14 ) 

=  0; 

%y_dot 

manual (15) 

=  0; 

%  z_dot 

manual (16) 

= 

0; 

%u  measurement 

manual  { 17 ) 

0; 

%v  measurement 

manual  (18) 

0; 

%w  measurement 

%EPF-A 

manual (19) 

= 

0; 

2- V 

0  X 

manual (20) 

= 

0; 

%y 

manual  (21) 

= 

0; 

%z 

manual (22) 

= 

0; 

%V 

manual (23) 

= 

0; 

%theta 

manual  (24) 

= 

0; 

%phi 

manual (25) 

= 

0; 

%dx 

manual (26) 

= 

0; 

%dy 

manual  (27) 

= 

0; 

%dz 

manual (28) 

= 

0; 

%dV 

manual (29) 

= 

0; 

%dtheta 

manual (30) 

= 

0; 

%dphi 

manual  (31) 

= 

0; 

%p_num 

manual  { 32 ) 

= 

0; 

%x  variance 

manual { 33 ) 

= 

0; 

%y  variance 

manual (34) 

= 

0; 

%z  variance 

manual { 35 ) 

= 

0; 

%V  variance 

manual (36) 

= 

0; 

%theta  variance 

manual  {31) 

= 

0; 

%phi  variance 

manual (38) 

= 

0; 

%dxp  variance 

variance 

variance 

variance 
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manual (39)  = 

0; 

%dyp  variance 

manual (40)  = 

0; 

%dzp  variance 

manual  (41)  = 

0; 

%dVp  variance 

manual  (42)  = 

0; 

%dthetap  variance 

manual  (43)  = 

0; 

%dphip  variance 

manual  (44)  = 

0; 

%Measurement  nois' 

manual  (45)  = 

1; 

%Filterl  u  weight 

manual (46)  = 

1; 

%Filterl  v  weight 

manual  (47)  = 

1; 

%Filterl  w  weight 

switch  target-Scenario 

case  1 

x=l;  y=0;  z=0;  V=4;  theta=90;  phi=0;  dx=0;  dy=0;  dz=0;  dV=0;. 
dtheta=0;  dphi=0;  xdot=0;  ydot=0;  zdot=0; 

case  2 

x=0;  y=l;  z=0;  V=4;  theta=0;  phi=0;  dx=0;  dy=0;  dz=0;  dV=0;.. 
dtheta=0;  dphi=0;  xdot=0;  ydot=0;  zdot=0; 

case  3 

x=0;  y=0;  z=l;  V=4;  theta=90;  phi=90;  dx=0;  dy=0;  dz=0;  dV=0; 
dtheta=0;  dphi=0;  xdot=0;  ydot=0;  zdot=0; 

case  4 

x=l;  y=0;  z=0;  V=4;  theta=90;  phi=180;  dx=0;  dy=0;  dz=0;  dV=0 
dtheta=0;  dphi=0;  xdot=0;  ydot=0;  zdot=0; 

case  5 

x=0;  y=l;  z=0;  V=4;  theta=180;  phi=0;  dx=0;  dy=0;  dz=0;  dV=0; 
dtheta=0;  dphi=0;  xdot=0;  ydot=0;  zdot=0; 

case  6 

x=0;  y=0;  z=5;  V=4;  theta=90;  phi=270;  dx=0;  dy=0;  dz=0;  dV=0 
dtheta=0;  dphi=0;  xdot=0;  ydot=0;  zdot=0; 
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case  7 


x=0;  y=0;  z=10;  V=5;  theta=90;  phi=0;  dx=0;  dy=0;  dz=0;  dV=0;... 
dtheta=l;  dphi=0;  xdot=0;  ydot=0;  zdot=0; 

case  8 

x=0;  y=0;  z=10;  V=5;  theta=90;  phi=0;  dx=0;  dy=0;  dz=0;  dV=0;... 
dtheta=0;  dphi=l;  xdot=0;  ydot=0;  zdot=0; 

case  9 

x=0;  y=0;  z=10;  V=5;  theta=0;  phi=90;  dx=0;  dy=0;  dz=0;  dV=0;... 
dtheta=l;  dphi=0;  xdot=0;  ydot=0;  zdot=0; 

case  10 

x=5;  y=5;  z=5;  V=l;  theta=45;  phi=45;  dx=0;  dy=0;  dz=0;  dV= . 1 ; . . . 
dtheta=5;  dphi=5;  xdot=0;  ydot=0;  zdot=0; 

case  0 

x=manual (1) ;  y=manual (2) ;  z=manual (3) ;  V=manual (4)  ;  .  .  . 
theta=manual (5) ; 

phi=inanual  (6)  ;  dx=manual  (7)  ;  dy=manual  ( 8 )  ;  dz=inanual  (9)  ;  .  .  . 
dV=manual (10) ; 

dtheta=manual ( 11 )  ;  dphi=manual { 12 ) ;  xdot=manual (13) ;  .  .  . 
ydot=manual (14) ;  zdot=manual (15) ; 
otherwise 

error (' INCORRECT  TARGET  SCENARIO  SELECTION') 

end 

switch  P_variance_scenario 
case  1 

TProcess_V= . 1 ;  TProces  s_theta=deg2rad (5 ) ;  TProcess_phi=deg2rad ( 5 ) ; 
case  2 

TProcess_V= . 0 1 ;  TProcess_theta= . 01 ;  TProcess_phi= . 01 ; 
case  0 

TProcess_V=0 ;  TProcess_theta=0;  TProcess_phi=0 ; 
otherwise 

error (' INCORRECT  PROCESS  VARIANCE  SCENARIO  SELECTION') 
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end 


switch  m_variance_scenario 
case  1 

Mu_var=0;  Mv_var=0;  Mw_var=0; 
case  2 

Mu_var=5;  Mv_var=5;  Mw_var=5; 
case  3 

Mu_var=10;  Mv_var=10;  Mw_var=10; 
case  0 

Mu_var=manual  ( 1 6 )  ;  Mv_var=inanual  { 17  )  ;  Mw_var=manual  ( 1 8 )  ; 
otherwise 

error (' INCORRECT  VARIANCE  SCENARIO  SELECTION') 

end 

switch  Kalman_ICs 
case  1 

xkl=x;  yk;I=y;  zkl  =  z;  Vkl=V;  thetakl=theta;  phikl=phi; 

dxkl=dx;  dykl=dy;  dzkl=dz;  dVkI=dV;  dthetakl=dtheta;  dphikl=dphi; 
case  2 

xkl=0;  ykl=0;  zkl=0;  Vkl=0;  thetakl=0;  phikl=0; 

end 

switch  Kalman_Control_Law 
case  1 

K_ux=  (dV/dt )  *sind  (thetakl )  *cosd  (phikl  )*(dt''2/2)+... 

(dtheta/dt)  *VkI*cosd  (thetakl)  *cosd(  phikl)  *  (dt''2/2)  +  .  .  . 

—  (dphi/ dt ) *Vkl*sind (thetakl ) *sind (phikl ) * (dt " 2/2 )  ; 

K_uy=(dV/dt)  *cosd  (thetakl )  *  (dt''2/2)-.  .  . 

(dtheta/dt)  *Vkl*sind  (thetakl)  *  (dt''2/2)  ; 
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K_uz=  (dV/dt )  *sind  (thetakl )  *sind  (phikl  )*(dt''2/2)+... 

(dtheta/dt)  *Vkl*cosd  (thetakl)  *sind  (phikl)  *  (dt''2/2)  +  .  .  . 
(dphi/dt)  *Vkl*sind  (thetakl)  *cosd(phikl)  *  (dt''2/2)  ; 


case  2 

K_ux=.5;  K_uy=.5;  K_uz=.5; 
case  3 

K_ux=l;  K_uy=l;  K_uz=l; 

end 

switch  Kalman_Process_Variance 
case  1 

KProcess_x=l ;  KProcess_y=l;  KProcess_z  =  l; 

KProcess_dx=TProcess_V*sind (TProcess_theta) *cosd (TProcess_phi ) *dt; 

KProcess_dy=TProcess_V*cosd (TProcess_theta) *dt; 

KProcess_dz=TProcess_V*sind (TProcess_theta) *sind (TProcess_phi ) *dt; 

end 

switch  KalmanJyieasureinent -Variance 
case  1 

KMu_var=Mu_var ;  KMv_var=Mv_var ;  KMw_var=Mw_var ; 

end 

switch  f ilterl-scenario 
case  1 

xpl=x;  ypl=y;  zpl=z;  Vpl=V;  thetapl=theta;  phipl=phi; 

dxpl=dx;  dypl=dy;  dzpl=dz;  dVpl=dV;  dthetapl=dtheta;  dphipl=dphi ; 
case  2 

xpl=0;  ypl=0;  zpl=0;  Vpl=0;  thetapl=0;  phipl=0; 

dxpl=0;  dypl=0;  dzpl=0;  dVpl=0;  dthetapl=0;  dphipl=0; 
case  3 
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xpl=0;  ypl=0;  zpl=15;  Vpl=0;  thetapl=0;  phipl=0; 
dxpl=0;  dypl=0;  dzpl=0;  dVpl=0;  dthetapl=0;  dphipl=0; 
case  0 

xpl=manual  { 1  9 )  ;  ypl=manual  (20  )  ;  zpl=inanual  (21)  ; 

Vpl=manual (22 ) ;  thetapl=manual (23) ;  phipl=manual (24 ) ; 
dxpl=manual (25)  ;  dypl=manual (2  6) ;  dzpl=manual (27 ) ; 
dVpl=inanual  (28)  ;  dthetapl=inanual  (29)  ;  dphipl=manual  (30)  ; 
otherwise 

error (' INCORRECT  FILTERl  INTIAL  CONDITIONS') 

end 

switch  f ilterl_variance 
case  1 

xpl_v=l;  ypl_v=l;  zpl_v=l;  Vpl_v=.5;  thetapl_v= . I;  phipl_v=.l 
dxpl_v=.5;  dypl_v=.5;  dzpl_v=.5;  dVpI_v=.3;  dthetapl_v= .  I ;  .  .  . 
dphipl_v= . I ; 

case  2 

xpl_v=l;  ypl_v=l;  zpl_v=l;  Vpl_v=l;  thetapl_v= . I ;  phipl_v=.l; 
dxpl_v=.5;  dypl_v=.5;  dzpl_v=.5;  dVpI_v=.I;  dthetapl_v= . 01 ;  .  . 
dphipl_v= . 01 ; 

case  0 

xpl_v=manual (32) ;  ypl_v=manual (33) ;  zpl_v=manual(34); 
Vpl_v=manual (35) ;  thetapl_v=manual (36)  ;  phipl_v=manual (37) ; 
dxpl_v=manual ( 38 ) ;  dypl_v=manual (39) ;  dzpl_v=manual(40); 
dVpI_v=manual ( 4 1 ) ;  dthetapl_v=manual ( 42 )  ;  dphipl_v=manual (43) 
otherwise 

error (' INCORRECT  FILTERl  STATE  VARIANCES') 

end 

switch  filterl_ineasureinent -Variance 
case  1 

ml  _v=Mu_var  ; 
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case  2 


ml  _v=  .  1 ; 
case  3 

ml  _v=  .  1 ; 
case  0 

ml_v=manual  (44) ; 
otherwise 

error (' INCORRECT  FILTERl  MEASUREMENT  VARIANCES') 

end 

switch  f ilterl_weight 
case  1 

W_u=l;  W_v=l;  W_w=l; 
case  0 

W_u=manual (45) ;  W_v=manual ( 4 6 )  ;  W_w=manual (47) ; 
otherwise 

error  (' INCORRECT  FILTERl  WEIGHTS') 

end 


target 

(1) 

=  x;  %x 

target 

(2) 

y;  %y 

target 

(3) 

=  z ;  %  z 

target 

(4) 

V;  % 

V 

target 

(5) 

=  theta 

;  %theta 

target 

(6) 

phi; 

%phi 

target 

(7) 

=  dx; 

%dx 

target 

(8) 

dy; 

%dy 

target 

(9) 

=  dz  ; 

%dz 

target 

(10) 

=  dV; 

%dV 

target 

(11) 

=  dtheta;  %dtheta 

target 

(12) 

=  dphi; 

%dphi 

target 

(13) 

=  xdot; 

%x_dot 
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target  (14) 
target (15) 
target (16)  = 

target  { 17 )  = 

target  (18)  = 

obs  ( 1 )  = 

obs  (2 )  = 

obs  ( 3 )  = 

Kf liter  ( 1 )  = 

Kfilter  (2)  = 

Kfilter{3)  = 
Kfilter { 4 )  = 

Kfilter{5)  = 
Kfilter (6)  = 

Kfilter  (7)  = 

Kfilter  (8)  = 

Kfilter  (9)  = 

Kfilter  ( 10 )  = 
Kfilter (11)  = 
Kfilter  ( 12 )  = 
Kfilter (13)  = 

Kfilter  { 14 )  = 

Kfilter  (15)  = 

Kfilter (16)  = 

Kfilter  { 17 )  = 

Kfilter (18)  = 

f ilterl  ( 1 )  = 

filterl  (2)  = 

filterl  (3)  = 


ydot;  %y_dot 
zdot;  %z_dot 
TProcess_V; 
TProcess-theta 
TProcess_phi; 


xkl  ; 
ykl; 
zkl  ; 

Vkl; 

thetakl ; 
phikl ; 

K_ux  ; 

K-Uy; 

K_uz  ; 

KProcess_x; 
KProcess_y; 
KProcess_z ; 
KProcess_dx; 
KProcess.dy  ; 
KProcess_dz  ; 
KMu_var ; 
KMv_var ; 
KMw_var ; 

xpl;  %x 

ypi;  %y 

zpl;  %z 


Mu_var  ; 
Mv_var  ; 
Mw_var ; 


f ilterl 

(4) 

= 

Vpl;  %V 

f ilterl 

(5) 

= 

thetapl;  % 

f ilterl 

(6) 

= 

phipl ; 

f ilterl 

(7) 

= 

dxp  1  ; 

f ilterl 

(8) 

= 

dypl; 

f ilterl 

(9) 

= 

dzpl  ; 

f ilterl 

(10) 

= 

dVp  1  ; 

f ilterl 

(11) 

= 

dthetapl ; 

f ilterl 

(12) 

= 

dphipl ; 

f ilterl 

(13) 

= 

p_num ; 

f ilterl 

(14) 

= 

xpl_v;  %x  variance 

f ilterl 

(15) 

= 

ypl_v;  %y  variance 

f ilterl 

(16) 

= 

zpl_v;  %z  variance 

f ilterl 

(17) 

= 

Vpl_v;  %V  variance 

f ilterl 

(18) 

= 

thetapl_v;  %theta  variance 

f ilterl 

(19) 

= 

phipl_v;  %phi  variance 

f ilterl 

(20) 

= 

dxpl_v;  %dxp  variance 

f ilterl 

(21) 

= 

dypl_v;  %dyp  variance 

f ilterl 

(22) 

= 

dzpl_v;  %dzp  variance 

f ilterl 

(23) 

= 

dVpl_v;  %dVp  variance 

f ilterl 

(24) 

= 

dthetapl_v;  %dthetap  variance 

f ilterl 

(25) 

= 

dphipl_v;  %dphip  variance 

f ilterl 

(26) 

= 

ml_v;  %Measurement  noise  covar. 

f ilterl 

(27) 

= 

W_u  ; 

f ilterl 

(28) 

= 

W_v  ; 

f ilterl 

(29) 

= 

W_w ; 

for  i=l:sim_run 
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[T_G_cent,  0_G_cent ,  0_C_cent,  K_G_S ,  P_G_S,  L_G_cent,  L_C_cent]  = 
EPF_A_Function_Final (params,  target,  obs,  Kfilter,  filterl); 
%Convert  to  degrees 

T_G_cent(:, [5,6,11,12]) =rad2deg (T_G_cent(:, [5,6,11,12])); 

P_G_S (:, [5, 6, 11, 12]) =rad2deg ( P _G_S {:, [5,6,11,12])); 

L_G_cent(:,  [8,9]) =rad2deg {L_G_cent { : ,  [8,9] ) ) ; 

K_G_S  (:,  [5, 6] )=rad2deg(K_G_S  (:,  [5,  6]  )  )  ; 

sim_T_G_cent  (:,  :,i)=T_G_cent; 
s  im_0_G_cent  (:,  :,i)=0_G_cent; 
sim_K_G_S  ( :  ,  :  ,  i) =K_G_S ; 
sim_P_G_S  (  :  ,  :  ,  i)  =P_G_S  ; 
s im_L_G_cent  (:,  :,i)=L_G_cent; 
i 

end 

switch  sim_plot 
case  1 

fig=l; 

hfig=figure (fig) ; 

set (hfig,  ' Position ',[  0 ,  0,  800,  800]);  %  [x  y  width  height] 

axis  equal 
xlabel ( ' X ' ) ; 
ylabel ( ' Y ' ) ; 
z label ( ' Z ' ) ; 
for  i2=4 : params ( 1 ) 
hold  on; 

scatters (T_G_cent (12, 1) , T_G_cent  (i2, 2) , T_G_cent  (12, 3) ,  'b' ) 
scatters  (0_G_cent  (12, 4) ,  0_G_cent  (i2, 5) ,  0_G_cent  (12, 6) ,  'k'  ) 
scatters (L_G_cent (12, 1) , L_G_cent (i2, 2) , L_G_cent (12, 3) , 'g' ) 
scatters (K_G_S  (12,1), K_G_S  (12,2),  K_G_S  (12,3),  'm' )  ; 
scatters (P_G_S  (12,1), P_G_S  (12,2), P_G_S  (12,3),  ' r '  )  ; 
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end 


legend ( ' Target '  ,  ' Observation '  ,  ' SLMA ' ,  ' Kalman ' ,  ' EPF— A ' ,  .  .  . 

' Location ' , ' East ' ) 
fig=f ig+1; 

%Comparison  of  States 
time=zeros (params (1) , 1) ; 
for  12=3 : params ( 1 ) 
time (12 , 1 ) =i2 ; 

end 

%Positions 
hfig=figure (fig) ; 

set (hfig,  ' Posit ion ',[  0 ,  0,  800,  1200]);  %  [x  y  width  height] 

a=subplot  (3,1,1); 

plot ( [time ( 1 ) , NaN] ,  [  T_G_cent  ( 1 , 1 ) , NaN] ,  ' *-b ' ,  ... 

[time (1)  ,NaN]  ,  [0_G_cent  (1, 1) ,NaN] ,  ' *-g ' ,  .  .  . 

[time (1) ,NaN] , [L_G_cent (1, 1) ,NaN] , '*-c' , . . . 

[time (1) ,NaN] , [K_G_S (1,1) ,NaN] , ' *-m' , . . . 

[time (1) ,NaN] ,  [P_G_S  (1,1)  ,NaN]  ,  ' *-r ' )  ; 
legend ( ' Target '  ,  ' Measurement '  ,  ' SLMA ' ,  ' EKF— A ' ,  ' EPF— A ' ,  . .  . 
'Location', 'North', 'Orientation', 'Horizontal') 
set ( legend,  'Orientation ' ,  ' horizontal ',' Posit ion ',..  . 

[0.3  .04  .4  .025]); 
hold  on; 

plot (time,  T_G_cent  (  :  , 1 ) ,  ' b ' ,  .  .  . 
time, 0_G_cent  (  :  , 4 )  ,  '  g  '  ,  .  .  . 
time, L_G_cent  (  :  , 1 ) ,  '  c  '  ,  .  .  . 
time, K_G_S (:,1), 'm', . . . 
time, P _G_S  ( :  ,  1 )  ,  '  r  '  )  ; 
hold  on; 

plot (time (l:20:end) , T_G_cent ( (l:20:end) , 1) , '*b', . . . 
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time (4:20:end) ,0_G_cent  (  (4:20: end)  ,  4 )  ,  ' *g '  , 
time (8:20:end) ,L_G_cent ( (8:20: end) , 1 ) , ' *c ' , 
time (12:20: end) ,K_G_S( (12:20: end) , 1 ) , ' *m ' , . 
time ( (16:20:end) ) , P_G_S ( (16:20:end) , 1) , ' *r' 
title  (a,  'x  Position'); 
xlabel  (a,  ' Time  Step'); 
ylabel  (a,  'Position' ) ; 

b=subplot (3,1,2); 
plot (time, T_G_cent  (  :  , 2 ) ,  ' b ' ,  .  .  . 
time, 0_G_cent ( : , 5 ) , ' g ' , . . . 
time,  L_G_cent  (  :  , 2 ) ,  ' c ' ,  .  .  . 
time, K_G_S (:,2), 'm', . . . 
time, P _G_S  ( :  ,  2 )  ,  '  r  '  )  ; 
hold  on; 

plot (time (l:20:end) , T_G_cent ( (l:20:end) ,2) , '*b' 
time (4:20:end) ,0_G_cent ( (4:20: end) , 5 ) , ' *g ' , 
time (8:20:end) ,L_G_cent  (  (8:20: end)  ,  2 )  ,  ' *c ' , 
time (12:20: end) ,K_G_S( (12:20: end)  ,  2 )  ,  ' *m ' ,  . 
time ( (16:20:end) ) , P_G_S ( (16:20:end) ,2) , '*r' 
title (b,'y  Position'); 
xlabel (b, ' Time  Step'); 
ylabel  (b,  'Position ' ) ; 

c=subplot  (3, 1,3); 
plot (time,  T_G_cent  (  :  , 3) ,  'b ' ,  .  .  . 
time,  0_G_cent  ( : , 6) ,  ' g ' ,  . . . 
time, L_G_cent  (  : , 3) ,  ' c ' ,  .  .  . 
time, K_G_S  (:,3),  'm',  .  .  . 
time, P _G_S  ( :  ,  3 )  ,  '  r  '  )  ; 
hold  on; 

plot (time (l:20:end) , T_G_cent ( (1 :20:end) , 3) , ' *b' 
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time (4:20:end) ,0_G_cent  (  (4:20: end)  ,6),'*g',... 
time (8:20:end) ,L_G_cent ( (8:20: end) ,3),'*c',... 
time (12:20: end) ,K_G_S( (12:20: end) ,3),'*m',... 
time ( (16:20:end) ) ,P_G_S ( (16:20:end) ,3) , '*r') ; 
title  (c,'z  Position'); 
xlabel  (c,  ' Time  Step'); 
ylabel (c, 'Position' ) ; 
fig=f ig+1; 

%Velocity  Magintude  and  headings 
hfig=figure (fig) ; 

set (hfig,  ' Position ',[  0 ,  0,  800,  1200]);  %  [x  y  width  height] 

a=subplot  (3,1,1); 

plot ( [time ( 1 ) , NaN] ,  [  T_G_cent  ( 1 , 1 ) , NaN] ,  ' *-b ' ,  ... 

[time (1) ,NaN]  ,  [L_G_cent  (1, 1) ,NaN] ,  '*-c' ,  .  . . 

[time (1) ,NaN] , [K_G_S (1,1) ,NaN] , ' *-m' , . . . 

[time (1) ,NaN] , [P_G_S (1,1) ,NaN] , ' *-r ' ) ; 
legend ( ' Target ' ,  ' SLMA ' ,  ' EKF— A ' ,  ' EPF— A '  ,  'Location '  ,  'North ' ,  .  .  . 
'Orientation', 'Horizontal') 

set ( legend,  'Orientation '  ,  ' horizontal ',' Position ',..  . 

[0.3  .04  .4  .025]); 
hold  on; 

plot (time, T_G_cent  (  :  , 4 ) ,  ' b '  ,  .  .  . 
time, L_G_cent ( : , 7 ) , ' c ' , . . . 
time, K_G_S  (:,4),  'm',  .  .  . 
time, P _G_S  ( :  ,  4 )  ,  '  r  '  )  ; 
hold  on; 

plot (time (l:16:end) , T_G_cent ( (l:16:end) , 4) , '*b', . . . 
time (4:16:end) ,L_G_cent ( (4:16: end) ,7),'*c',... 
time (8:16:end) , K_G_S  ( (8:16: end)  ,  4 )  ,  ' *m '  ,  .  .  . 
time ( (12 : 16: end) ) , P_G_S ( (12 : 16: end) , 4) , ' *r ' ) ; 
title  (a,  'Velocity  Magnitude ' ) ; 
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xlabel  (a,  ' Time  Step'); 
y label (a, 'Velocity ' ) ; 

b=subplot  (3,1,2); 
plot  (time,  T_G_cent  (  :  , 5) ,  'b ' ,  .  .  . 
time,  L_G_cent  (  :  , 8) ,  ' c ' ,  .  .  . 
time,  K_G_S  (:,5),  'm',  .  .  . 
time, P _G_S  ( :  ,  5 )  ,  '  r  '  )  ; 
hold  on; 

plot (time (l:16:end) , T_G_cent ( (l:16:end) ,5) , '*b', 
time (4:16:end) ,L_G_cent ( (4:16: end) , 8 ) , ' *c ' , . 
time (8:16:end) , K_G_S  ( (8:16: end)  ,  5 )  ,  ' *m '  ,  .  . . 
time ( (12 : 16: end) ) , P_G_S  (  (12 : 16:end) , 5) ,  ' *r  '  ) 
title  (b,  ' Theta  Heading'); 
xlabel (b, ' Time  Step ' ) ; 
ylabel (b,  ' Heading  (deg)  '  )  ; 

c=subplot (3,1,3); 
plot  (time, T_G_cent  (  :  , 6) ,  ' b '  ,  ... 
time, L_G_cent  (  : , 9) ,  ' c  '  ,  .  .  . 
time, K_G_S(:,6),'m',... 
time, P _G_S  ( :  ,  6 )  ,  '  r  '  )  ; 
hold  on; 

plot (time (l:16:end) , T_G_cent ( (l:16:end) , 6) , '*b', 
time (4:16:end) ,L_G_cent ( (4:16: end) , 9 ) , ' *c ' , . 
time (8:16:end) , K_G_S  ( (8:16: end)  ,  6 )  ,  ' *m '  ,  .  . . 
time ( (12 : 16: end) ) , P_G_S  (  (12 : 16:end) , 6) ,  ' *r '  ) 
title  (c,  'Phi  Heading' ) ; 
xlabel (c, ' Time  Step'); 
ylabel (c,  ' Heading  (deg)  ' )  ; 
fig=f ig+1; 
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%%  Mean  Errors 
for  i=2 : T 

Error_SLMA  ( i,  :  )  =abs  (L_G_cent{i,  [1,2,3,7,8,9])-... 

T_G_cent  (i,  [1, 2, 3, 4, 5, 6] ) ) ; 

Error_Kalman (i,  : ) =abs (K_G_S  (i,  :)-T_G_cent  (i,  [1,2, 3, 4, 5, 6] ) ) ; 
Error_Particle(i, :) =abs (P_G_S (i, [1,2, 3, 4, 5, 6])-. . . 

T_G_cent  (1,  [1, 2, 3, 4, 5, 6] ) ) ; 

end 

%Return  Mean  Errors 
SLMA=inean{Error_SLMA{T-25:T,  :  )  ) 

Kalinan=mean  (Error.Kalman  (T— 25  :  T,  :  )  ) 

Particle=mean (Error .Particle (T— 25 : T, : ) ) 

otherwise 

message='WILL  NOT  PLOT,  MULTIPLE  SIMULATION  RUNS' 

Calculate  performace  abilities 

%NOTE :  Although  distance  and  V  errors  are  absolute,  heading  errors 
%can  only  be  up  to  180  degrees  off  for  both  phi  and  theta  (ie.  you 
%are  pointinig  in  the  complete  opposite  direction.  Theta  is 
%already  between  0  and  180,  so  aboslute  errors  may  be  taken.  Phi 
%though  must  be  adjusted  so  that  all  errors  are  between  0  and  180 
%(ex.  an  'error'  of  350  is  really  only  an  error  of  10 
%'Best'  Values  for  filter  and  comparison 
range_index=solution_range*  s im.run ; 
for  i=2 : params ( 1 ) 

for  i2=l:sim_run 

%Create  variable  vectors 

t_dist=sqrt  {  sim_T_G_cent  {i,l,i2)  "'2+.  .  . 

sim_T_G_cent  (i,2,i2)  ''2  +  sim_T_G_cent  (i,3,i2)  "'2)  ; 
k_dist=sqrt  {sim_K_G_S  (i,l,i2)  ''2+sim_K_G_S  (i,2,i2)  “2+.  .  . 
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sim_K_G_S  {i,3,i2)''2); 

f _dist=sqrt  {sim_P_G_S  (i,l,i2)  ''2+sim_P_G_S  (i,2,i2)  "'2+. 

siin_P_G_S  {i,3,i2)''2); 
c_dist=sqrt { s  im_L_G_cent (1,1, i2)  "2+ .  .  . 

sim_L_G_cent  (i,2,i2)  ''2  +  sim_L_G_cent  (i,3,i2)  "'2)  ; 

vec_siin_K_G_S  {i2, 1)  =abs  {k_dist-t_dist)  ; 
vec_siin_K_G_S{i2,  [2,3,4])  =abs  (sim_K_G_S(i,  [4,5,6],i2) 
sim_T_G_cent (i, [4,5, 6] ,12) ) ; 
vec_siin_P_G_S  (12, 1)  =abs  {f_dist-t_dist)  ; 
vec_siin_P_G_S{i2,  [2,3,4])  =abs  (sim_P_G_S(i,  [4, 5,  6], 12) 
slin_T_G_cent  (1,  [4,5,  6]  ,12)  )  ; 
vec_s lm_L_G_cent ( 12 , 1 ) =abs ( c_dlst-t _dlst ) ; 
vec_s  lm_L_G_cent  (12,  [2,3,4]  )=.  .  . 

abs ( s lm_L_G_cent (1, [7, 8, 9], 12)-... 
slin_T_G_cent  (1,  [4,5,  6]  ,12)  )  ; 

%Phl  adjustment 
If  vec_slm_K_G_S (12, 4) >180 

vec_slm_K_G_S  (12,4)=3  60— vec_slm_K_G_S  (12,4)  ; 

end 

If  vec_slm_P_G_S  (12,  4)  >180 

vec_slm_P_G_S  (12,4)=360— vec_slm_P_G_S  (12,4)  ; 

end 

If  vec_slm_L_G_cent  (12, 4) >180 

vec_s  lm_L_G_cent  (12, 4)  =360— vec_slm_L_G_cent  (12, 4) 

end 

end 

svec_slm_K_G_S=sort  (vec_slm_K_G_S)  ; 
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svec_sim_P_G_S=sort  (vec_sim_P_G_S)  ; 
svec_s  im_L_G_cent=sort  (vec_sim_L_G_cent )  ; 
error_t_k  (i,  :  )  =svec_s  im_K_G_S  ( range_index ,  :  )  ; 
error_t_f  (i,  :  )  =svec_s  im_P_G_S  ( range-index ,  :  )  ; 
error-t_c  (i,  :  )  =svec_sim_L_G_cent  (range_index,  :  )  ; 

end 

temp_error_k=mean (error_t_k (params (1) —50 :params (1)  ,  : ) ) ; 
temp-error-f =mean (error-t_f (params (1) -50 :params (1) , : ) ) ; 


%Mean  or  average  error  values  for  filter  and  comparison 
for  i=2 : params ( 1 ) 

for  i2=l:sim_run 

%Create  variable  vectors 

t_dist=sqrt  {  sim-T_G_cent  {i,l,i2)  "'2+.  .  . 

sim_T_G_cent  (i,2,i2)  ''2  +  sim_T_G_cent  (i,3,i2)  "'2)  ; 
k-dist=sqrt  {sim-K_G_S  (i,l,i2)  ''2+sim-K-G_S  (i,2,i2)  "'2+. 
sim_K_G_S  {i,3,i2)''2); 

f _dist=sqrt  (sim_P_G_S  (i,l,i2)  ''2+sim_P_G_S  (i,2,i2)  "'2+. 

sim_P_G_S  (i,3,i2)''2); 
c_dist=sqrt  {  sim_L_G_cent  {i,l,i2)  "'2+.  .  . 

sim-L-G_cent  (i,2,i2)  ''2  +  sim_L-G_cent  (i,3,i2)  "'2)  ; 

vec_sim_K-G_S (12,1) =abs {k_dist-t_dist) ; 
vec_sim_K_G_S(i2,  [2,3,4]) =abs (sim_K_G_S(i,  [4,5, 6], i2) 
sim-T-G_cent (i, [4,5, 6] ,i2) ) ; 
vec_sim_P_G_S (12, 1) =abs {f_dist-t_dist) ; 
vec_sim_P_G_S{i2,  [2,3,4])  =abs  (sim_P_G_S(i,  [4,5,6],i2) 
sim-T-G_cent (i, [4,5, 6] ,i2) ) ; 
vec_s im_L_G_cent ( i2 , 1 ) =abs ( c_dist-t _dist ) ; 
vec_s  im_L-G_cent  (i2,  [2,3,4]  )=.  .  . 
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abs ( s im_L_G_cent (i, [7, 8, 9], 12)-... 
siin_T_G_cent  (i,  [4,5,  6]  ,12)  )  ; 

If  vec_slm_K_G_S (12, 4) >180 

vec_slm_K_G_S  (12,4)=360— vec_slm_K_G_S  (12,4)  ; 

end 

If  vec_slm_P_G_S  (12,  4)  >180 

vec_slm_P_G_S  (12,4)=360— vec_slm_P_G_S  (12,4)  ; 

end 

If  vec_slm_L_G_cent  (12, 4) >180 

vec_s  lm_L_G_cent  (12, 4)  =360— vec_slm_L_G_cent  (12, 4)  ; 

end 

end 

error_m_k  (1,  1)  =mean  (vec_slm_K_G_S  (:,!)); 
error_m_k  (1,2)  =mean  ( ve  c_s  lm_K_G_S  (  :  ,  2)  )  ; 
error_m_k  (1,  3)  =mean  ( ve c_s  lm_K_G_S  (  :  ,  3)  )  ; 
error_m_k  (1,  4)  =mean  (vec_slm_K_G_S  (  :  ,  4 )  )  ; 

error_m_f  (1,  1)  =mean  (vec_slm_P_G_S  (:,!)); 
error_m_f  (1,2)  =mean  (vec_slm_P_G_S  (  :  ,  2)  )  ; 
error_m_f  (1,  3)  =mean  (vec_slm_P_G_S  (  :  ,  3)  )  ; 
error_m_f  (1,  4)  =mean  (vec_slm_P_G_S  (  :  ,  4 )  )  ; 

error_m_c (1,  1) =mean ( vec_slm_L_G_cent  (:,!)); 
error_m_c (1,2) =mean ( vec_slm_L_G_cent  (  :  , 2) )  ; 
error_m_c  (1,3)  =mean  (vec_slin_L_G_cent  (  :  ,  3)  )  ; 
error_m_c  (1,  4)  =mean  (vec_slin_L_G_cent  (  :  ,  4)  )  ; 

end 
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%Bias  (Overall  mean  for  certain  range  versus  simulation  number) 

bias_start=params { 1 ) -50  ; 

bias_stop=params (1)  ; 

sim_runx=5; 

for  i=l:sim_run 

for  i2=bias_start : bias_stop 

t_dist=sqrt  {  sim_T_G_cent  (12, 1,1)  ''2  +  .  .  . 

sim_T_G_cent  (12,2,1)  ''2  +  sim_T_G_cent  (12,2,1)  '2)  ; 
k_dist=sqrt  (sim_K_G_S  (12,1,1)  ''2+sim_K_G_S  (12,2,1)  "'2+.  .  . 
sim_K_G_S  (i2,2,i)''2); 

f _dist=sqrt  (sim_P_G_S  (12,1,1)  ''2+sim_P_G_S  (12,2,1)  "'2+.  .  . 

sim_P_G_S  (i2,2,i)''2); 
c_dist=sqrt  (  sim_L_G_cent  (12, 1,1)  "'2+.  .  . 

sim_L_G_cent  (12,2,1)  ''2  +  sim_L_G_cent  (12,2,1)  "'2)  ; 

vec2_sim_K_G_S (12, 1) =k_dist—t_dist; 

ve c2 _s im_K_G_S (12,  [2,3,4]  )=sim_K_G_S  (12,  [4,5, 6] ,1)-.  .  . 

sim_T_G_cent  (12,  [4,5,6]  ,1)  ; 
vec2_sim_P_G_S (12, 1) =f_dist—t_dist; 

ve c2 _s im_P_G_S (12,  [2,3,4]  )=sim_P_G_S  (12,  [4,5, 6] ,1)-.  .  . 

sim_T_G_cent  (12,  [4,5,6]  ,1)  ; 
vec2_sim_L_G_cent  (12,  1)  =  c_dist— t_dist; 
vec2_sim_L_G_cent  (12,  [2,3,4]  )=.  .  . 

sim_L_G_cent  (12,  [7,  8,  9]  ,  i)-sim_T_G_cent  (12,  [4, 5,  6]  ,  i)  ; 

if  vec2_sim_K_G_S (12, 4) >180 

vec2  _s  im_K_G_S  (12, 4)  =360— vec2_sim_K_G_S  (12,4)  ; 

end 

if  vec2_sim_P_G_S (12, 4) >180 

vec2_sim_P_G_S  (12, 4)  =360— vec2_sim_P_G_S  (12,4)  ; 

end 
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if  vec2_sim_L_G_cent  (i2, 4) >180 

vec2_s  im_L_G_cent  (i2, 4)  =360— vec2_sim_L_G_cent  (i2,  4)  ; 

end 

if  vec2_sim_K_G_S  (i2, 4) <— 180 

vec2_siin_K_G_S  (i2,4)  =abs  (vec2_sim_K_G_S  (12,4)  )— 360; 

end 

if  vec2_sim_P_G_S  (12, 4) <— 180 

vec2_siin_P_G_S  (12,4)  =abs  (vec2_sim_P_G_S  (12,4)  )— 360; 

end 

if  vec2_sim_L_G_cent  (12, 4) <— 180 
vec2_s im_L_G_cent  (12,4)=.  .  . 

abs ( vec2_sim_L_G_cent (12,4) )-360; 

end 

end 

temp-bias-k  (1,1)  =mean  (vec2_sim_K_G_S  (  :  ,  1)  ,  1)  ; 

temp_bias_k  (1,2)  =mean  (vec2_sim_K_G_S  (  :  ,  2)  ,  1)  ; 

temp_bias_k  (1,3)  =mean  (vec2_sim_K_G_S  (  :  ,  3)  ,  1)  ; 

temp_bias_k  (1,4)  =mean  (vec2_sim_K_G_S  (  :  ,  4)  ,  1)  ; 

temp_bias_p  (1,1)  =mean  (vec2_sim_P_G_S  (  :  ,  1)  ,  1)  ; 

temp_bias_p  (1,2)  =mean  (vec2_sim_P_G_S  (:,2)  ,1)  ; 
temp_bias_p  (1,3)  =mean  (vec2_sim_P_G_S  (  :  ,  3)  ,  1)  ; 

temp_bias_p  (1,4)  =mean  (vec2_sim_P_G_S  (  :  ,  4)  ,  1)  ; 

temp_bias_c  (1,1)  =mean  (vec2_siin_L_G_cent  (  :  ,  1)  ,  1)  ; 
temp_bias_c  (1,2)  =mean  (vec2_siin_L_G_cent  (  :  ,  2)  ,  1)  ; 
temp_bias_c (1,3) =mean (vec2_sim_L_G_cent ( : , 3) , 1) ; 
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temp_bias_c (i,  4) =mean (vec2_sim_L_G_cent ( : , 4 ) , 1 ) ; 
count=i 

end 

time=zeros (params (1) , 1) ; 
for  12=2 : params ( 1 ) 
time (12 , 1 ) =12 ; 

end 

fig=l; 

%TER 

hf ig=f igure ( ' name ' , ' Threshold  Error ' ) ; 

set (hfig,  ' Position ',[ 0 ,  0,  800,  1200]);  % [x  y  width  height] 
a=subplot (4,1,1); 
plot (time, error _t_c (:,1)  ,  'g'  ,  .  .  . 
time,  error_t_k ( : , 1) ,  'm' ,  .  .  . 
time,  error  _t_f(:,l),  'r'); 

legend ( ' Comparison ' , ' Kalman ' , ' EPF— A ' , 'Location ' , ' NorthEast ' ) 
title (a, ' Distance  Error'); 
xlabel  (a,  ' Time  Step'); 
ylabel (a, ' Error ' ) ; 

b=subplot  (4,1,2); 
plot  (time,  error_t_c(:,2),  'g',  ... 
time,  error_t_k(:,2),  'm',  .  .  . 
time,  error _t_f (:,2)  ,  'r'  ,  .  .  . 
time, T_G_cent  (  : , 4 ) ,  ' b  '  ) ; 

legend ( ' Comparison ' , ' Kalman ' , ' EPF— A ' , ' Target ' , . . . 

'Location', 'NorthEast') 
title (b, 'Velocity  Error'); 
xlabel (b, ' Time  Step ' ) ; 
ylabel (b, 'Error' ) ; 
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c=subplot  (4,1,3); 
plot  (time,  error_t_c(:,3),  'g',  .  .  . 
time,  error_t_k:  (  :  ,  3 )  ,  ' m '  ,  .  .  . 
time,  error_t_f(:,3),  'r',  .  .  . 
time, T_G_cent ( : , 5 ) , 'b ' ) ; 

legend ( ' Compare ion ' , ' Kalman ' , ' EPF— A ' , ' Target ' , . . . 

'Location', 'NorthEast') 
title (c, ' Theta  Heading  Error'); 
xlabel  (c,  ' Time  Step'); 
ylabel (c, 'Error  (Deg) ' ) ; 

d=subplot  (4,1,4); 
plot  (time,  error_t_c(:,4),  'g',  .  .  . 
time,  error_t_k ( : , 4) ,  'm' ,  .  .  . 
time,  error _t_f (:,4)  ,  'r'  ,  .  .  . 
time, T_G_cent  (  : , 6) ,  'b '  ) ; 

legend ( ' Comparison ' , ' Kalman ' , ' EPF— A ' , 'Target ' , . . . 

'Location', 'NorthEast') 
title (d, 'Phi  Heading  Error'); 
xlabel (d, ' Time  Step'); 
ylabel (d, 'Error  (Deg) ' ) ; 
f ig=f ig+1 ; 

%MAE 

hf ig=f igure ( ' name ' , ' Mean  Error ' ) ; 

set (hfig,  ' Position ',[ 0 ,  0,  800,  1200]);  %  [x  y  width  height] 
a=subplot  (4,1,1); 
plot (time, error_m_c (  : ,  1 )  ,  '  g '  ,  .  .  . 
time,  error_m_k  (  :  ,  1)  ,  'm'  ,  .  .  . 
time,  error_m_f {:,!),  '  r  '  )  ; 

legend ( ' Comparison '  ,  ' Kalman ' ,  ' EPF— A ' ,  'Location ' ,  ' NorthEast ' ) 
title (a, ' Distance  Error'); 
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xlabel  (a,  ' Time  Step'); 
ylabel (a, ' Error ' ) ; 

b=subplot  (4,1,2); 
plot (time, error_m_c (  : ,  2  )  ,  '  g  '  ,  ... 
time,  error_m_k:  (  :  ,  2)  ,  'm'  ,  .  .  . 
time,  error_m_f (  ;  ,  2 )  ,  '  r  '  ,  .  .  . 
time, T_G_cent  (  : , 4 ) ,  ' b  '  ) ; 
legend ( ' Comparison ' , ' Kalman ' , ' EPF— A ' , ' Target ' , 
'Location', 'NorthEast') 
title (b, 'Velocity  Error'); 
xlabel (b, ' Time  Step ' ) ; 
ylabel (b, 'Error' ) ; 

c=subplot  (4,1,3); 
plot (time, er ror_m_c (  : ,  3 )  ,  '  g '  ,  .  .  . 
time,  error_m_k:  (  :  ,  3 )  ,  ' m '  ,  .  .  . 
time,  error_m_f (  :  ,  3 )  ,  '  r '  ,  .  .  . 
time, T_G_cent  (  : , 5 ) ,  'b '  ) ; 
legend ( ' Comparison ' , ' Kalman ' , ' EPF— A ' , 'Target ' , 
'Location', 'NorthEast') 
title (c, ' Theta  Heading  Error'); 
xlabel (c, ' Time  Step'); 
ylabel (c, 'Error  (Deg) ' ) ; 

d=subplot (4,1,4); 
plot (time, error_m_c (  : ,  4 )  ,  '  g '  ,  .  .  . 
time,  error_m_k  (  :  ,  4)  ,  'm'  ,  .  .  . 
time,  error_m_f (  :  ,  4 )  ,  '  r  '  ,  .  .  . 
time, T_G_cent  (  : , 6) ,  'b '  ) ; 
legend ( ' Comparison ' , ' Kalman ' , ' EPF— A ' , ' Target ' , 
'Location', 'NorthEast') 
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title (d, 'Phi  Heading  Error'); 
xlabel (d, ' Time  Step'); 
ylabel (d, 'Error  (Deg) ' ) ; 
f ig=f ig+1 ; 

%Final  50  step  metrics 

best_error_c=mean ( error  _t_c (params (1) —50 :params (1) ,  : ) ) 
best_error_k=mean ( error  _t_k (params (1) —50 : params (1) ,  : ) ) 
best_error_f =mean (error_t_f (params (1) -50 :params (1) , : ) ) 

%Final  50  step  metrics 

mean_error_c=mean (error_m_c (params (1) —50 :params (1)  ,  : ) ) 
mean_error_k=mean (error_m_k (params (1) —50 :params (1) , : ) ) 
mean_error_f =mean (error_m_f (params (1) -50 :params (1) , : ) ) 

end 

A.4.2  EPF  A.  J' unction  J'inal. 

function  [T_G_cent,  0_G_cent,  0_C_cent,  K_G_cent,  P_G_S,  L_G_cent,... 
L_C_cent]  =  Particle_Filter_A_Function_l (params,  target,  obs,... 
Kfilter,  filterl) 

%%  This  function  is  Particle  Filter  A,  allowing  for  multiple  scenarios 
%%  simulation  runs.  Inputs  are  contained  within  a  separate  .mfile  that 
%%  executes  this  file. 

%%  Initial  conditions  and  setup 

rng (' shuffle ' )  %Shuffle  random  numbers 

%System  parameters 

T  =  params (1);  %Number  of  iterations 

dt  =  params (2);  %Time  step 


or 
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%  Rotation  matrix  between  global  and  camera  frames 
Rot_y=@ (a)  [cos (a) ,  0,  -sin{a);  0,1,0;  sin(a),  0,  cos (a) ]  ; 

Rot_x=@ (a)  [ 1, 0,  0;  0,  cos (a)  ,  -sin(a);  0,sin(a),  cos (a) ] ; 

Rot_z=@ (a)  [cos (a) , -sin (a) , 0; sin (a) ,  cos{a),0;0,0,l]; 

PlotDCM=@ (A, 0)  plots (cumsum {[0(1),A(1,1)]), cumsum {[0(2),A(2,1)]),... 
cumsum ([0(3),A{3,1)]), 'r-', . . . 

cumsum { [0(1) ,A(1,2) ] ) , cumsum { [0(2) ,A(2,2) ] ) , cumsum ([0(3),A(3,2)]), 'g-', 
cumsum ( [0(1) ,A(1,3) ] ) , cumsum ( [0(2) ,A(2,3) ] ) , cumsum ([0(3),A(3,3)]),  'b-', 
' linewidth ' , 5 ) ; 

%%  Target  parameters 


%Truth 

conditions,  the  inital  conditions  of  the  centroid 

X  = 

target ( 1 )  ; 

Y 

target (2 ) ; 

z  = 

target ( 3 ) ; 

V 

=  target ( 4 ) ; 

theta 

=  deg2rad (target ( 5 ))  ; 

phi 

=  deg2rad (target ( 6) )  ; 

dx  = 

target ( 7 ) ; 

dy  = 

target ( 8 ) ; 

dz  = 

target ( 9) ; 

dV 

=  target (10); 

dtheta 

=  deg2rad (target (11)  )  ; 

dphi 

=  deg2rad (target ( 12 ))  ; 

xdot 

=  target (13) ; 

ydot 

=  target ( 14 ) ; 

zdot 

=  target (15) ; 

T_G_cent(l, :)=[x  y  z  V  theta  phi  dx  dy  dz  dV  dtheta  dphi  xdot  ydot  zdot]; 

%Process  Noise 
TS_V (1) =target (16)  ; 


187 


TS_V (2) =target (17); 
TS_V (3) =target (18)  ; 


%P real location 
T_C_cent  =  zeros (T,  12)  ; 
T_G_Rotdata=zeros (T,  3)  ; 
T_RotG2C=zeros (3, 3,  T)  ; 


%%  Measurement  Parameters 
%lnitial  point  locations 
0_G_point_a=  [  T_G_cent  (1)- 
0_G_point_b=  [  T_G_cent  (1)  + 
0_G_point_c=  [  T_G_cent  (1)- 
0_G_point_d= [ T_G_cent  (1)- 
0_G_point_e=  [  T_G_cent  (1)- 
0_G_point_f = [ T_G_cent  (1)  + 
0_G_point_g=  [  T_G_cent  (1)  + 
0_G_point_h=  [  T_G_cent  (1)  + 


(for  a  cube) ,  based  of  T  centroid 


.5  T_G_cent (2 ) — . 5 
.  5  T_G_cent  (2 ) - . 5 
.  5  T_G_cent  (2)  +  . 5 
.  5  T_G_cent (2 ) - . 5 
.  5  T_G_cent  (2)  +  .  5 
.  5  T_G_cent  (2 ) - . 5 
.  5  T_G_cent  (2)  +  .  5 
.  5  T_G_cent  (2)  +  .  5 


T_G_cent  (3) -  .  5] ; 
T_G_cent  ( 3 ) - . 5 ] ; 
T_G_cent  ( 3 ) - . 5 ] ; 
T_G_cent ( 3 ) + . 5 ] ; 
T_G_cent ( 3 ) + . 5 ]  ; 
T_G_cent ( 3 ) + . 5 ]  ; 
T_G_cent ( 3 ) - . 5 ]  ; 
T_G_cent ( 3 ) +  .  5 ]  ; 


%Form  the  initial  cube  based  of  points 

0_G_points=  [0_G_point_a;  0_G_point_b;  0_G_point_f ;  0_G_point_h;  .  .  . 
0_G_point_g;  0_G_point_c ;  0_G_point_a;  0_G_point_d;  .  .  . 
0_G_point  _e  ;  0_G_point  _h ;  0_G_point  _f  ;  0_G_point  _d;  .  .  . 
0_G_point  _e  ;  0_G_point  _c  ;  0_G_point  _g ;  0_G_point  _b  ]  ; 


%Number  of  points 
0_num  =  16; 


%P re alio cat ion 
0_G_cent=zeros (T, 6) ; 
0_C_cent=zeros (T, 3) ; 
0_RotG2C=zeros (3, 3,  T)  ; 
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0_C_points=zeros  (0_num, 3, T) ; 


%Measurement  noise  covariances 
0_V_M(1)  =  obs(l); 

0_V_M(2)  =  obs(2); 

0_V_M(3)  =  obs(3); 

%Match  inital  conditions 

0_G_cent  (1,  : )  =  [T_G_cent  (1,  [1,2,3]),  T_G_cent  (1,  [1,2,3])]; 

%%  Kalman  Parameters 
%Control  law 

K_u= [Kf liter ( 7 ) ;  Kfilter(8);  Kf liter ( 9)  ]  ; 

%State  transition  model  matrix 
K_A= [1  0  0  dt  0  0; . . . 

0  1  0  0  dt  0 ;  .  .  . 

0  0  1  0  0  dt ;  .  .  . 

000100;... 

000010;... 

000001]; 

%Control  mode  input 
K_B=  [dt''2/2  0  0;  .  .  . 

0  dt''2/2  0;  .  .  . 

0  0  dt''2/2;  .  .  . 
dt  0  0 ;  .  .  . 

0  dt  0 ;  .  .  . 

0  0  dt ] ; 

%Observation  model 
K_C=  [1  0  0  0  0  0;  .  .  . 

010000;... 

001000]; 

%Measurement  noise  variance  values 
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K_VM= [Kf ilter ( 1 6 )  Kfilter(17)  Kf liter ( 1 8 ) ] ; 

%Process  Noise  Covariance 
K_Ex= [Kfilter (10)  0  0  0  0  0;... 

0  Kfilter (11)  0  0  0  0;  .  .  . 

0  0  Kfilter  (12)  0  0  0;  .  .  . 

0  0  0  Kfilter (13)  0  0;  .  .  . 

0  0  0  0  Kfilter (14)  0;  .  .  . 

0  0  0  0  0  Kfilter  (15) ] ; 

%Measurement  noise  variance  matrix 
K_Em= [Kfilter ( 1 6 )  0  0;... 

0  Kfilter (17)  0; . . . 

0  0  Kfilter (18) ]  ; 

%Estimate  of  initial  target  position  variance  matrix 
K_P=K_Ex; 

%Initial  states 

xkl=Kf  ilter  ( 1 )  ;  yk;l=Kf  ilter  ( 2  )  ;  zkl=Kf  ilter  ( 3 )  ; 

Vkl=Kf ilter ( 4 ) ;  thetakl=Kf ilter ( 5 ) ;  phikl=Kf ilter ( 6) ; 
%Linearize  initial  conditions 
KS (1,  : )  =  [xkl  ykl  zkl .  .  . 

Vkl *sind (thetakl ) *cosd (phikl ) *dt . . . 

Vkl*cosd (thetakl) *dt . . . 

Vkl*sind (thetakl) *sind(phikl) *dt] ; 

%%  Particle  A  Parameters  (U,V,W) 

%Initial  conditions 


xpl  = 

f ilterl ( 1 ) ; 

ypl 

f ilterl (2 ) ; 

zpl  = 

filterl  (3) ; 

Vpl 

=  filterl  ( 4 ) ; 

thetapl 

=  filterl  (5) ; 

phipl 

=  filterl  (  6 ) ; 
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dxpl  = 

filterl (7)  ; 

dypl 

f ilterl ( 8 ) ; 

dzpl  = 

filterl ( 9) ; 

dVpl 

filterl  (10) ; 

dthetapl 

filterl(ll); 

dphipl 

filterl  (12) ; 

xdotpl  = 

Vpl*sin (thetapl) *cos (phipl) *dt; 

ydotpl  = 

Vpl*cos (thetapl) *dt; 

zdotpl  = 

Vpl*sin (thetapl) *sin (phipl) *dt; 

P_G_S (1, : ) = [xpl  ypl  zpl  Vpl  thetapl  phipl  dxpl  dypl  dzpl  dVpl  dthetapl . . . 
dphipl  xdotpl  ydotpl  zdotpl]; 


%Number  of  particles 


P_nuin  = 

filterl  (13) ; 

%Variances 

P_V_S (1) 

= 

f ilterl  ( 14 ) ;  %x  variance 

P_V_S  (2) 

= 

f ilterl  ( 15 ) ;  %y  variance 

P_V_S (3) 

= 

f ilterl  ( 1 6 ) ;  %z  variance 

P_V_S (4) 

= 

f ilterl  ( 17 ) ;  %V  variance 

P_V_S (5) 

= 

f ilterl  ( 18 ) ;  %theta  variance 

P_V_S  (6) 

= 

f ilterl  ( 1 9 ) ;  %phi  varaince 

P_V_S (7) 

= 

filterl  (20 ) ;  %dxp 

P_V_S  (8) 

= 

filterl (21 ) ;  %dyp 

P_V_S (9) 

= 

filterl  (22 ) ;  %dzp 

P_V_S (10) 

= 

filterl (23 ) ;  %dVp 

P_V_S  (11) 

= 

filterl  (24 ) ;  %dthetap 

P_V_S (12) 

= 

filterl (25 ) ;  %dphip 
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P_V_M  =  filterl(26);  %Measureinent  noise  covariance 


%Weighing 

matrix 

W_u 

= 

filterl (27)  ; 

%u 

observation 

weight 

W_v 

= 

filterl (28)  ; 

%v 

observation 

weight 

W_w 

= 

filterl (29) ; 

%w 

observation 

weight 

P_W  =  [W_u  0  0;  0  W_v  0;  0  0  W_w]; 

%Initial  particles  for  global  state 

P_G_S_part=zeros  (P_num, 15, T) ;  %Preallocation  of  particle  matrix 
for  i2=l : P_num 

P_G_S_part  (i2,  : , 1)  =  [P_G_S  (1, 1) tsqrt {P_V_S  (1)  ) *randn;  .  .  . 
P_G_S  (1, 2) +sqrt (P_V_S  (2) ) *randn;  .  .  . 

P_G_S  (1, 3)  tsqrt  (P_V_S  (3)  )  *randn;  .  .  . 

P_G_S  (1, 4) tsqrt (P_V_S  (4) ) *randn;  .  .  . 

P_G_S (1, 5) tsqrt (P_V_S (5) ) *randn; . . . 

P  _G_S  ( 1 , 6) tsqrt ( P  _V_S  { 6 ) ) *randn;  .  .  . 

P_G_S (1, 7) tsqrt (P_V_S (7) ) *randn; . . . 

P_G_S (1, 8) tsqrt (P_V_S (8) ) *randn; . . . 

P_G_S  (1, 9) tsqrt (P_V_S  (9) ) *randn;  .  .  . 

P_G_S  (1, 10) tsqrt (P_V_S  (10) ) *randn;  .  .  . 

P_G_S  (1, 11) tsqrt (P_V_S  (11) ) *randn;  .  .  . 

P_G_S  (1, 12) tsqrt (P_V_S  (12) ) *randn;  .  .  . 

P_G_S_part (i2, 10, 1) ; . . . 

P_G_S_part  (i2, 11, 1) ;  .  .  . 

P_G_S_part (12,12,1)]; 

end 

%P re alio cat ion 

P_G_S_part_u=zeros (P_num, 15, T) ; 
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P_RotG2C=zeros (3,3,T); 

P_C_0_part=zeros  (P_num, 3, T) ; 

P_P_0_dif f=zeros (P_num, 3, T) ;  %NEED  TO  CHANGE  BACK  TO  5  if  not  W 
P_P_W=zeros (T, P_num) ; 

%%  SLMA 

L_G_cent=zeros (T, 9) ; 

L_C_cent  (1,  [  1 , 2 , 3 ] ) =0_C_cent  { 1 ,  [1,2,3]); 

L_C_cent(l,  [4,5,6])  =  [0,0,0]; 

for  i=2:T  %i=l  is  initial  conditions 
%%  Truth  Centroid 

%Update  truth  centroid  position 

%Position  values  will  NOT  be  future  values:  the  x  used  here  is  based 
%off  of  the  previous  x  plus  the  previous  delta_x.  Thus,  delta  and  dot 
%values  pertain  to  the  x  in  the  same  time  step  NOT  a  future  x 
x=T_G_cent  (i-1, 1) +T_G_cent  (i-1, 7) ; 
y=T_G_cent (i-1, 2) +T_G_cent (i-1, 8) ; 
z=T_G_cent (i-1, 3) +T_G_cent (i-1, 9) ; 

V=T_G_cent (i-1, 4) +T_G_cent (i-1, 10) ; 
theta=T_G_cent (i-1, 5) +T_G_cent  (i-1, 11) ; 
phi=T_G_cent (i-1, 6) +T_G_cent  (i-1, 12)  ; 

xdot=V*sin (theta) *cos (phi) *dt; 
ydot=V*cos (theta) *dt; 
zdot=V*sin (theta) *sin (phi) *dt; 

V_dot=T_G_cent (i-1, 10) /dt; 
theta_dot=T_G_cent (i-1, 11) /dt; 
phi_dot=T_G_cent (i-1, 12) /dt; 

dx=V*sin (theta) *cos (phi) *dt+ . . . 
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V_dot  *sin  (theta)  *cos{phi)*(dt''2/2)+.  .  . 
theta_dot*V*cos  (theta)  *cos  (phi)  *  (dt''2/2)  + 
— phi_dot *V*sin  (theta)  *sin  (phi)  *  (dt''2/2)  ; 

dy=V*cos (theta) *dt+. . . 

V_dot *cos  (theta)  *  (dt''2/2)-.  .  . 
theta_dot*V*sin  (theta)  *  (dt''2/2)  ; 

dz=V*sin (theta) *sin (phi) *dtt .  .  . 

V_dot  *sin  (theta)  *sin  (phi  )*(dt''2/2)+... 
theta_dot*V*cos  (theta)  *sin  (phi)  *  (dt''2/2)  + 
phi_dot*V*sin  (theta)  *cos  (phi)  *  (dt''2/2)  ; 

dV=V_dot  *dt ; 
dtheta=theta_dot  *dt ; 
dphi=phi_dot  *dt  ; 

%Add  Process  Noise 
V=V+sqrt (TS_V (1) ) *randn; 
theta=theta+sqrt (TS_V (2) ) *randn; 
phi=phi+sqrt (TS_V (3) ) *randn; 

%New  centroid  positions 
T_G_cent  (i, 1) =x; 

T_G_cent  (i, 2) =y; 

T_G_cent  (i,3)=z; 

T_G_cent (i, 4) =V; 

T_G_cent  (i, 5) =theta; 

T_G_cent  (i, 6) =phi; 

T_G_cent  (i, 7) =dx; 

T_G_cent  (i, 8) =dy; 

T_G_cent  (i, 9) =dz; 
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T_G_cent  (i, 10) =dV; 

T_G_cent  (i, 11) =dtheta; 

T_G_cent  (i, 12) =dphi; 

T_G_cent  (i, 13) =xdot; 

T_G_cent  (1, 14) =ydot; 

T_G_cent  (i, 15) =zdot; 

%Rotate  to  camera  orientation 

%States  in  the  camera  orientation  are  u,v,w  and  dots 
%Generate  angles  and  rotation  DCM 

%Camera  angles  assume  the  camera  tracks  the  truth  centroid  perfectly 
T_G_Rotdata  (1,1)  =sqrt  (T_G_cent  (1,1)  ''2  +  T_G_cent  (1,2)  ''2  +  T_G_cent  (1,3)  "2)  ; 
%Pan  (phi) 

T_G_Rotdata  (1,2) =atan2  (T_G_cent  (i, 3) ,  T_G_cent  (i, 1) ) ; 

%Tilt,  note:  tilts  from  y  axis  (positive  down),  so  0  is  on  y— axis 
% (theta) 

T_G_Rotdata  (i,3)=acos  (T_G_cent  (1,2)  /T_G_Rotdata  (1,1)  )  ; 

%Calculate  rate  of  rotation  (phi  dot) ,  x 

T_G_Rotdata  (i,  4)  =  (T_G_Rotdata  (i,  2)  -T_G_Rotdata  (i-1, 2)  )  /dt; 

%Calculate  rate  of  tilt  (theta  dot) ,  y 

T_G_Rotdata  (i,  5)  =  (T_G_Rotdata  (i,  3)  -T_G_Rotdata  (i-1, 3)  )  /dt; 

%Create  DCM  based  on  current  rotation  angles 

T_DCM_G2C=@ (angz)  Rot_y (angz (1) —pi/ 2) *Rot_x (angz (2) —pi/2) *Rot_z (pi/ 2) ; 
T_RotG2C  (  :  ,  : , i ) =T_DCM_G2 C (T_G_Rotdata (i,  [2, 3] ) )  .  ' ; 

%Rotate  centroid  to  camera  frame 

T_C_cent(i, [1,2,3]) =T_RotG2C (:,:,i)*T_G_cent(i, [1,2,3]) %u,v,w 
T_C_cent(i,  [4,5,6]) =T_RotG2C (:,  :,i)*T_G_cent  (i,  [13,14,15])  %dot  u  v  w 
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9-  9-- 
o  o 


Observation  Centroid  and  Points 


%Update  global  point  position  (CUBE) 
for  12=1 :  0_nuin 

0_G_points  (12, 1, i) =0_G_points  (12, 1, i-1) +T_G_cent  (i-1, 7) ; 
0_G_points  (i2,2,i)=0_G_points  (i2,2,i-l)+T_G_cent  (i-1, 8) ; 
0_G_points  (i2, 3, i) =0_G_points  (12, 3, i-1) +T_G_cent  (i-1, 9) ; 

end 

%Calculate  observed  centroid  based  on  mean  of  points 
0_G_cent  (i, 1) =mean (0_G_points ( : , 1, i) ) ; 

0_G_cent  (i, 2) =mean (0_G_points ( : , 2, i) ) ; 

0_G_cent  (i, 3) =mean (0_G_points ( : , 3, i) ) ; 

%Rotate  to  camera  orientation  (use  same  cam  rot  angles  as  in  truth) 
%Rotate  points  to  camera  orientation 
0_RotG2C  (  :  ,  : , i) =T_RotG2C ( : ,  : , i) ; 

O_noise= [sqrt (0_V_M (1)  ) *randn;  sqrt (0_V_M  (2)  ) *randn;  .  .  . 

sqrt (0_V_M (3) ) *randn] ; 
for  12=1 : 0_num 

0_C -points (i2,  :,i) =0_RotG2C ( : ,  : , i) *0_G_points  (i2,  :,i)  '+O_noise; 

end 

%Calculate  observed  centroid  (in  camera  frame) 

0_C_cent  (i, 1) =mean (0_C_points ( : , 1, i) ) ; 

0_C_cent (i, 2) =mean (0_C_points ( : , 2, i) ) ; 

0_C_cent  (i, 3) =mean (0_C_points ( : , 3, i) ) ; 

T_RotC2G  (  : ,  : , i) =transpose (T_RotG2C ( : ,  : , i) ) ; 

0_G_cent(i,  [4,5,6]) =T_RotC2G  (:,:,i)*0_C_cent(i,  [1,2,3]) 

%%  Kalman  Filter 

KS_t  =  K-A  *  KS(i-l,:)'  +  K-B*K-U; 
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KS  (i,  :  )  =KS_t  '  ; 


K_P  =  K_A*K_P*K_A  '  +K_Ex  '  ; 

K_K  =  K_P*K_C  '  *inv  (K_C*K_P*K_C  '+K_Ein)  ; 

%Rotate  observations  from  camera  to  global 
%  K_comp ( 1 ,  [1,2,3]) =T_RotC2G (:,  ;,i)*0_C_cent  (1,  [1,2,3]) 

%Update  state  estimate 

KS_t=KS (1, : ) ' +K_K* {T_G_cent (1, [1,2,3]) '-K_C*KS (1, : ) ' ) ; 

KS  (1,  :  )  =KS_t  '  ; 

%Update  covariance  estimation 
K_P=  (eye  (6)  -K_K*K_C)  *K_P  ; 

%Convert  to  global  states 
K_G_cent  (i, 1) =KS (i, 1) ; 

K_G_cent ( i , 2 ) =KS { i , 2 ) ; 

K_G_cent  ( i , 3 ) =KS { i , 3 ) ; 

%  K_G_cent  (i,  [1,2,3]) =KS ([1,2,3]); 

K_G_cent  (i,  4)  =abs  (sqrt  (KS  (i,  1)  '2+KS  (i,  2)  ''2+KS  (i,  3)  "2)  )  ; 
K_G_cent (i, 5) =acos (KS (i, 5) /K_G_cent  (i, 4) ) ; 

K_G_cent  (i, 6) =atan2 (KS (i, 6)  ,  KS (i,  4) )  ; 

%%  Particle  Centroid  and  Points  (U,V,W) 

%Update  particles  (state  and  observations) 

%As  with  the  Truth  cent,  take  in  the  previous  time  step  values 
for  12=1 : P_num 

%Update  state 

x=P_G_S_part  (i2, 1, i— 1) +P_G_S_part  (i2, 7,  i— 1)  ; 
y=P_G_S_part  (i2, 2, i-1) +P_G_S_part  (i2, 8,  i-1)  ; 
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z=P_G_S_part (i2, 3, i-1) +P_G_S_part (12,9,1-1); 

V=P_G_S_part  (12,4,l-l)+P_G_S_part  (12,10,1-1) +sqrt ( P  _V_S  ( 4 ) ) *randn; 
theta=P _G_S_part  (12,5,l-l)+P_G_S_part  (12,11,1-1)+.  .  . 
sqrt (P_V_S (5) ) *randn; 

phl=P _G_S_part  (12,6,1— 1)+P_G_S _p art  (12,12,1-1)+.  .  . 
sqrt (P_V_S (6) ) *randn; 

%Angle  adjustment:  ensure  V  Is  not  negative,  angles  In  proper  range 
If  (V<0) 

V=abs (V) ; 

end 

%Flx  and  reduce  angles 
theta=wrapTo2Pl (theta)  ; 

If (theta>pl ( ) ) 

theta_a=theta-pl () ; 
theta=pl ( ) -theta_a ; 

end 

xdot=V*sln (theta) *cos (phi) *dt; 
ydot=V*cos (theta) *dt; 
zdot=V*sln (theta) *sln (phi) *dt; 

V_dot=T_G_cent (1-1, 10) /dt; 
theta_dot=T_G_cent (1-1, 11) /dt; 
phl_dot=T_G_cent (1-1, 12) /dt; 

dx=V*sln (theta) *cos (phi) *dt+ . . . 

V_dot*sln  (theta)  *cos  (phi)  *(dt''2/2)  +  .  .  . 
theta.dot  *V*cos  (theta)  *cos(phl)*(dt''2/2)  +  .  .  . 

-phl_dot  *V*sln  (theta)  *sln  (phi)  *  (dt''2/2)  ; 
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dy=V*cos (theta) *dt+ . . . 

V_dot *cos  (theta)  *(dt''2/2)-.  .  . 
theta_dot*V*sin  (theta)  *  (dt''2/2)  ; 

dz=V*sin (theta) *sin (phi) *dt+ . . . 

V_dot*sin  (theta)  *sin  (phi)  *(dt''2/2)  +  .  .  . 
theta.dot  *V*cos  (theta)  *sin(phi)*(dt''2/2)  +  .  .  . 
phi_dot*V*sin  (theta)  *cos  (phi)  *  (dt''2/2)  ; 

dV=V_dot  *dt  ; 
dtheta=theta_dot  *dt  ; 
dphi=phi_dot  *dt ; 

%New  centroid  positions 
P_G_S_part_u  (i2,l,i)=x; 

P_G_S_part_u  (i2,2,i)=y; 

P_G_S_part_u  (i2,3,i)=z; 

P_G_S_part_u (i2, 4, i) =V; 

P_G_S_part_u (i2, 5, i) =theta; 

P_G_S_part_u (i2, 6, i) =phi; 

P_G_S_part_u (i2, 7, i) =dx; 

P_G_S_part_u (i2, 8, i) =dy; 

P_G_S_part_u  (i2,9,i)=dz; 

P_G_S_part_u (i2, 10, i) =dV; 

P_G_S_part_u (i2, 11, i) =dtheta; 

P_G_S_part_u (i2, 12, i) =dphi ; 

P_G_S_part_u (i2, 13, i) =xdot ; 

P_G_S_part_u (i2, 14, i) =ydot; 

P_G_S_part_u (i2, 15, 1) =zdot; 

%Observation  Update  (ie.  what  we  think  the  camera  will  see  based  on 
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%the  states) 


%Rotate  to  camera  frame  (use  same  angles  as  in  Truth) 

P_RotG2C  (  :  ,  :  ,  i)  =T_RotG2C  (:,:,!); 

P_C_0_part  (12,  [1,2,3] , i) =P_RotG2C ( : ,  : , 1) *  .  .  . 

P_G_S_part_u (12,  [1,2,3],!)  '  ; 

%Calculate  difference  between  observation  (measurement)  and  filter 
%predicition 

P_P_0_diff  (12,  :,i)=0_C_cent  (1,  :)-P_C_0_part  (12,  :,i)  ; 

%Weights  to  be  used 

P_P_0_rawW  (i,i2)=P_P_0_diff  (12,  :,i)*P_W*P_P_0_diff  (12,  :,i) 

%Weight  particles 

P_P_W  (i,i2)  =  (l/ sqrt (2*pi*P_V_M) ) *exp (-(P_P_0_diff (12,  :,i)*P_W*.  .  . 
P_P_0_dif f  (12,  : , 1)  ' ) /  (2*P_V_M) ) ; 

end 

%Normalize  to  form  a  probability  distribution  (ie.  sums  to  1) 

P_P_W  (i,  : ) =P_P_W (i,  : )  . /sum(P_P_W (1,  : ) ) ; 

%Resampling:  from  this  new  distribution,  we  randomly  resample  from  it 
%to  generate  new  estimate  particles 
for  12=1 : P_num 

P  _P_get  (1,12) =f ind ( rand<=cumsum ( P  _P  _W (1,  : ) ) , 1 ) ; 

P_G_S_part  (12,  :,i)=P_G_S_part_u  (P_P_get  (1,12)  ,  :,i)  ; 

end 

%The  final  estimate,  state,  is  a  metric  of  the  final  resampling 
P_G_S  (i,  : ) =mean (P_G_S_part  (  :  ,  :  ,  1)  )  ; 

SLMA 
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0_C_cent(l,  [l,2,3])=0_G_cent(l,  [1,2,3]); 


L_C_cent  (i, 1) 
L_C_cent (1,2) 
L_C_cent  (1,3) 


0_C_cent (i-1, 1) +L_C_cent (i-1, 4) *dt; 
0_C_cent  (i-l,2)+L_C_cent  (i-1, 5) *dt; 
0_C_cent  (i-l,3)+L_C_cent  (i-l,6)*dt; 


%Velocities 
L_C_cent (1,4) 
L_C_cent (1,5) 
L_C_cent  (1, 6) 


(0_C_cent  (i, l)-0_C_cent  (i-1, 1) ) /dt; 
(0_C_cent  (i,2)-0_C_cent  (i-1, 2) ) /dt; 
(0_C_cent  (1, 3) -0_C_cent  (i-1, 3) ) /dt; 


%Rotate  to  global  from  camera  (inverse  of  DCM) 

L_G_cent  (1,  [1,2,3]) =T_RotC2G ( : ,  : , i) *L_C_cent  (i,  [1,2,3])  ' ; 


L_G_cent  (i,4)  =  (L_G_cent  (i,l)-L_G_cent  (1-1,1)  )/dt; 

L_G_cent  (i,5)  =  (L_G_cent (i,2)-L_G_cent (1-1,2) )/dt; 

L_G_cent  (i, 6)  =  (L_G_cent (i,3)-L_G_cent (i-1, 3) ) /dt; 

V  =  sqrt  (L_G_cent  (i,  4) ''2  +  L_G_cent  (1,  5) ''2  +  L_G_cent  (1,  6)  "2)  ; 

theta  =  acos ( L_G_cent ( i , 5 ) /V)  ; 

phi  =  atan2  (  L_G_cent  ( i , 6 ) ,  L_G_cent  ( 1, 4 ) ) ; 

L_G_cent  (1,  7)  =  V; 

L_G_cent  (1, 8)  =  theta; 

L_G_cent  (i, 9)  =  phi; 

end 


%Angle  adjustment:  ensure  V  is  not  negative,  angles  in  proper  range 
for  i=2 : T 

%Target 

T_G_cent  (i, 5) =wrapTo2Pi (T_G_cent  (i,  5)  )  ; 

T_G_cent  (i, 6) =wrapTo2Pi (T_G_cent  (i, 6) ) ; 
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if{T_G_cent  (i,4)<0) 

T_G_cent  (i, 4) =abs  (T_G_cent  (i, 4) ) ; 

T_G_cent  { i , 6 ) =wrapTo2Pi ( T_G_cent { i , 6 ) +pi ( ) ) ; 
T_G_cent  {i,5)=pi{)-T_G_cent  (i,5)  ; 

end 

%Fix  and  reduce  angles 
if{T_G_cent  (1,5) >pi ( ) ) 

theta _a=T_G_cent  (l,5)-pi()  ; 

T_G_cent { 1 , 5 ) =pi { ) -theta.a ; 

T_G_cent  { 1 , 6 ) =wrapTo2Pi ( T_G_cent { i , 6 ) +pi ( ) )  ; 

end 

%Kalman 

phi=K_G_cent (1, 6) ; 
if  (phi  >  2*pi  { ) ) 

mult=f loor (phi/ (2*pi { ) ) ) ; 
phi=phi— mult*2*pi  () ; 

end 

if  (phi  <  — 2*pi()) 

mult=floor{phi/ {-2*pi {) ) )  ; 
phi=phi+mult*2*pi ()  ; 

end 

if  (phi  <  0) 

phi=2*pi 0  +phi; 

end 

K_G_cent  (i, 6) =abs (phi) ; 

%Fix  and  reduce  angles 
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theta=K_G_cent  (i, 5) ; 
theta=wrapTo2Pi (theta) ; 
if {theta>pi ( ) ) 

theta_a=theta-pi ()  ; 
theta=pi ( ) -theta_a; 

end 

K_G_cent  (i, 5) =theta; 

%EPF-A 

%Only  need  to  adjust  phi,  v  is  already  only  absolute  and  theta  has 

%already  been  constrained 

%D0  NOT  USE  WRAPT02PI! ! ! 

phi=P_G_S  (i, 6) ; 

if  (phi  >  2*pi  { ) ) 

mult=f loor (phi/ (2*pi { ) ) ) ; 
phi=phi— mult*2*pi  () ; 

end 

if  (phi  <  -2*pi ( ) ) 

mult=floor{phi/ {-2*pi  () ) ) ; 
phi=phi+mult*2*pi  () ; 

end 

if  (phi  <  0) 

phi=2*pi 0  +phi; 

end 

P_G_S (i, 6) =abs (phi) ; 

%SLMA 

%Angle  adjustment:  ensure  V  is  not  negative,  angles  in  proper  range 
phi=L_G_cent  (i, 9) ; 
if  (phi  >  2*pi  { ) ) 
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mult=f loor (phi/ (2*pi { ) ) ) ; 
phi=phi— mult*2*pi ()  ; 

end 

if  (phi  <  -2*pi ( ) ) 

mult=floor{phi/ {-2*pi  {) ) ) ; 
phi=phi+mult*2*pi  () ; 

end 

if  (phi  <  0) 

phi=2*pi 0  +phi; 

end 

L_G_cent  (i, 9) =abs (phi) ; 

%Fix  and  reduce  angles 

theta=L_G_cent  (i, 8) ; 

theta=wrapTo2Pi (theta) ; 

if (theta>pi ( ) ) 

theta_a=theta-pi ()  ; 
theta=pi ( ) -theta.a; 

end 

L_G_cent  (i, 8) =theta; 

end 

A.5  Evaluated  Particle  Filter  B 

Two  functions  executed  and  plotted  EPF-B,  EPF-A,  SEMA,  and  SEMB.  The  first 
funetion,  EPFJBJExecuteJFinal.m,  provides  the  initial  conditions  to  the  seeond  function, 
EPF_B_Function_Final.ni,  which  executes  the  filters. 

A.5.1  EPF  _B  .Execute  J'inal. 
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%%  This  script  analyzes  EPF— B 
clear  all;  close  all;  clc; 

%%  Defaults 

%  Change  default  axes  fonts 

set ( 0 Default AxesFontName Times  New  Roman'); 
set ( 0 ,  ' Def aultAxesFontSize '  ,  12  )  ; 

%  Change  default  text  fonts 

set ( 0 ,  ' Def aultTextFontName '  ,  'Times  New  Roman') 
set ( 0 ,  ' Def aultTextFont Size '  ,  12 )  ; 

%%  Parameters 

%Number  of  simulation  runs 
sim_run=l  ; 
sim_plot=sim_run; 

%Threshold  Error  Range 
solution_range  =  .9; 

%Iternation  Number 
params { 1 )  =  5 ; 

%Time  Steps 
params (2)  =  . 1 ; 

%Focal  length 
params (3)  =  2000; 

%%  Target  Parameters 
%Straight  Line,  x— axis:  1 
%Straight  Line,  y— axis:  2 
%Straight  Line,  z— axis:  3 
%Straight  Line,  -x— axis:  4 
%Straight  Line,  -y— axis:  5 
%Straight  Line,  -z— axis:  6 
%Circle,  x/y,  about  z:  7 
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%Circle,  x/z,  about  y: 
%Circle,  y/z,  about  x: 
%Filter  B: 

%MANUAL,  see  below: 
target_scenario  =  10; 

%Process  Variance 
%0 . 1 :  2 
%None :  0 

P_variance_scenario  =  2; 


%No  measurment  variance:  1 
%5  Measurement  variance:  2 
%10  Measurement  variance:  3 
%MANUAL :  0 
m_variance_scenario  =2; 


1 

2 

3 

3 

3 

0 


%Number  of  particles 
p_num  =  500; 
p2_num  =  1500; 

%Weights  EPF-A 


%%  EPF-B/EPF-A  Parameters 
%With  target  intial  conditions: 
%Without,  at  0  (x,y,z) : 

%For  circle,  x/y  about  z: 

%For  circle,  x/z  about  y: 

%For  circle,  y/z  about  y: 
%MANUAL : 

f ilterl_scenario  =  0; 
f ilter2_scenario  =  0; 


8 

9 

10 
0 
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%A11  equal;  1 
%MANUAL :  0 

f ilterl_weight  =1; 

%Weights  EPF-B 
%A11  equal;  1 
%MANUAL ;  0 

f ilter2_weight  =  0; 

%Filter  variances 
%Scenario  1 ;  1 

%MANUAL ;  0 

f ilterl_variance  =  0; 
f ilter2_variance  =  0; 


%Measurment  Variance 
%Match  measurment ;  1 

%Variance  of  .01;  2 

%Variance  of  .1;  3 


%MANUAL ;  0 

%N0TE;  CANNOT  BE  0 
f ilterl_measurement_variance  =  1; 
f ilter2_measurement_variance  =  1; 


%Type  of  correction 
%Noise  on  vector; 

% Jacobian ; 

%No  correction; 
correction=0 ; 


to  use  for  filter  2 
1 
2 
0 


%Manual  values 
%Target 
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manual  { 1 ) 

0; 

%x 

manual { 2 ) 

= 

0; 

%y 

manual  { 3 ) 

= 

5; 

%z 

manual { 4 ) 

= 

1; 

%V 

manual  { 5 ) 

= 

90; 

%theta 

manual { 6) 

= 

0; 

%phi 

manual { 7 ) 

= 

0; 

%dx 

manual { 8 ) 

= 

0; 

%dy 

manual { 9) 

= 

0; 

%dz 

manual  (10) 

= 

0; 

%dV 

manual (11) 

= 

0; 

%dtheta 

manual { 12 ) 

= 

0; 

%dphi 

manual  (13) 

= 

0; 

%x_dot 

manual (14) 

= 

0; 

%y_dot 

manual  (15) 

= 

0; 

%z_dot 

manual (16) 

= 

0; 

%u  measurement 

variance 

manual  { 17 ) 

0; 

%v  measurement 

variance 

manual (18) 

0; 

%w  measurement 

variance 

%EPF-A 

manual (19) 

= 

10; 

9- Y 

0  X 

manual (20) 

= 

10; 

%y 

manual  (21) 

= 

10; 

%z 

manual (22) 

= 

5; 

%V 

manual (23) 

= 

10; 

%theta 

manual  (24) 

= 

10; 

%phi 

manual (25) 

= 

0; 

%dx 

manual (26) 

= 

0; 

%dy 

manual (27) 

= 

0; 

%dz 

manual (28) 

= 

0; 

%dV 
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manual (29) 

0;  %dtheta 

manual (30) 

= 

0;  %dphi 

manual (31) 

= 

500;  %p_num 

manual { 32 ) 

= 

.1;  %x  variance 

manual  { 33 ) 

= 

.1;  %y  variance 

manual  { 34 ) 

= 

.1;  %z  variance 

manual { 35 ) 

= 

.5;  %V  variance 

manual (36) 

= 

.5;  %theta  variance 

manual  {31) 

= 

.5;  %phi  variance 

manual (38) 

= 

.1;  %dxp  variance 

manual (39) 

= 

.1;  %dyp  variance 

manual (40) 

= 

.1;  %dzp  variance 

manual  (41) 

= 

.1;  %dVp  variance 

manual (42) 

= 

.1;  %dthetap  variance 

manual  (43) 

= 

.1;  %dphip  variance 

manual (44) 

= 

.1;  %Measurement  nois 

manual (45) 

= 

1;  %Filterl  u  weight 

manual (46) 

= 

1;  %Filterl  v  weight 

manual  (47) 

= 

1;  %Filterl  w  weight 

%EPF-B 

manual (48) 

= 

10;  %x 

manual (49) 

= 

10;  %y 

manual  (50) 

= 

10;  %z 

manual (51) 

= 

5;  %V 

manual  { 52 ) 

= 

10;  %theta 

manual  { 53 ) 

= 

10;  %phi 

manual (54) 

= 

0 ;  %  dx 

covariance 


209 


manual { 55 ) 

0 ;  %  dy 

manual (56) 

= 

0;  %dz 

manual  { 57 ) 

= 

0;  %dV 

manual  (58) 

= 

0;  %dtheta 

manual (59) 

= 

0;  %dphi 

manual { 60 ) 

= 

1000;  %p_num 

manual { 61 ) 

= 

.1;  %x  variance 

manual { 62 ) 

= 

.1;  %y  variance 

manual  { 63 ) 

= 

.1;  %z  variance 

manual { 64 ) 

= 

.5;  %V  variance 

manual { 65 ) 

= 

.5;  %theta  variance 

manual (66) 

= 

.5;  %phi  variance 

manual { 67 ) 

= 

.1;  %dxp  variance 

manual { 68 ) 

= 

.1;  %dyp  variance 

manual (69) 

= 

.1;  %dzp  variance 

manual  (70) 

= 

.1;  %dVp  variance 

manual (71) 

= 

.1;  %dthetap  variance 

manual  { 72 ) 

= 

.1;  %dphip  variance 

manual  {13) 

= 

30;  %Measurement  nois 

manual (74) 

= 

1;  %6  %1  %100  (to  see 

manual { 75 ) 

= 

1;  %6  %1 

manual (76) 

= 

1;  %3  %10 

manual  (77) 

= 

1;  %4  %10 

manual (78) 

= 

1;  %4  %10 

covariance 


this  affects 


the  weight 


value ) 


switch  target_scenario 
case  1 
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x=l;  y=0;  z=0;  V=4;  theta=90;  phi=0;  dx=0;  dy=0;  dz=0;  dV=0;.. 
dtheta=0;  dphi=0;  xdot=0;  ydot=0;  zdot=0; 

case  2 

x=0;  y=l;  z=0;  V=4;  theta=0;  phi=0;  dx=0;  dy=0;  dz=0;  dV=0;... 


dtheta=0;  dphi=0 

;  xdot=0; 

ydot=0 ; 

zdot=0 ; 

case  3 

x=0 ; 

II 

o 

N 

II 

O 

< 

II 

theta=90 ; 

phi=90 ; 

dx=0;  dy=0 

o 

II 

> 

o 

II 

N 

dtheta=0;  dphi=0 

;  xdot=0; 

ydot=0 ; 

zdot=0 ; 

case  4 

x=l; 

II 

> 

O 

II 

N 

O 

II 

>1 

theta=90 ; 

phi=180 ; 

dx=0;  dy=0 

o 

II 

> 

o 

II 

N 

dtheta=0;  dphi=0 

;  xdot=0; 

ydot=0 ; 

zdot=0 ; 

case  5 

x=0 ; 

II 

N 

II 

O 

< 

II 

theta=180; 

phi=0 ; 

dx=0;  dy=0; 

O 

II 

> 

o 

II 

N 

dtheta=0;  dphi=0 

;  xdot=0; 

ydot=0 ; 

zdot=0 ; 

case  6 

x=0 ; 

II 

o 

N 

II 

O 

< 

II 

theta=90 ; 

phi=270 

II 

>1 

o 

II 

X 

O 

II 

> 

Xi 

O 

II 

N 

x 

o 

dtheta=0;  dphi=0 

;  xdot=0; 

ydot=0 ; 

zdot=0 ; 

case  7 

x=0 ; 

LO 

II 

> 

O 

Csl 

II 

N 

O 

II 

>1 

theta=90 ; 

phi=0 ; 

dx=0;  dy=0; 

o 

II 

> 

X 

o 

II 

N 

X> 

dtheta=l;  dphi=0 

;  xdot=0; 

ydot=0 ; 

zdot=0 ; 

case  8 

x=0 ; 

II 

o 

N 

II 

O 

< 

II 

Cn 

theta=90 ; 

phi=0 ; 

dx=0;  dy=0; 

dz=0 ;  dV=0 ; . 

dtheta=0;  dphi=l 

;  xdot=0; 

ydot=0 ; 

zdot=0 ; 

case  9 

x=0 ; 

II 

o 

N 

II 

O 

< 

II 

cn 

theta=0 ; 

phi=90 ; 

dx=0;  dy=0; 

dz=0 ;  dV=0 ; . 

dtheta=l;  dphi=0 

;  xdot=0; 

ydot=0 ; 

zdot=0 ; 

case  10 


x=10;  y=10;  z=10;  V=5;  theta=10;  phi=10;  dx=0;  dy=0;  dz=0;... 
dV=.2;  dtheta=.5;  dphi=.5;  xdot=0;  ydot=0;  zdot=0; 

case  0 

x=manual  (1)  ;  y=inanual  (2)  ;  z=manual  (3)  ;  V=manual  (4)  ;  .  .  . 
theta=manual (5) ; 
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phi=manual { 6 ) ;  dx=manual ( 7 ) ;  dy=manual ( 8 ) ;  dz=manual { 9) ; . . . 
dV=manual (10)  ; 

dtheta=manual ( 1 1 )  ;  dphi=manual { 12 ) ;  xdot=manual ( 13 ) ;  .  .  . 
ydot=manual (14) ;  zdot=manual (15) ; 
otherwise 

error (' INCORRECT  TARGET  SCENARIO  SELECTION') 

end 

switch  P_variance_scenario 
case  1 

TProcess_V= . 1 ;  TProces  s_theta=deg2rad (5 ) ;  TProcess_phi=deg2rad ( 5 ) ; 
case  2 

TProcess_V= . 0 1 ;  TProcess_theta= . 01 ;  TProcess_phi= . 01 ; 
case  0 

TProcess_V=0 ;  TProcess_theta=0;  TProcess_phi=0 ; 
otherwise 

error (' INCORRECT  PROCESS  VARIANCE  SCENARIO  SELECTION') 

end 

switch  m_variance_scenario 
case  1 

Mu_var=0;  Mv_var=0;  Mw_var=0; 
case  2 

Mu_var=5;  Mv_var=5;  Mw_var=5; 
case  3 

Mu_var=10;  Mv_var=10;  Mw_var=10; 
case  0 

Mu_var=manual ( 1 6 ) ;  Mv_var=manual ( 17 ) ;  Mw_var=manual ( 1 8 ) ; 
otherwise 

error (' INCORRECT  VARIANCE  SCENARIO  SELECTION') 

end 
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switch  f ilterl_scenario 


case  1 

xpl=x;  ypl=y;  zpl=z;  Vpl=V;  thetapl=theta;  phipl=phi; 
dxpl=dx;  dypl=dy;  dzpl=dz;  dVpl=dV;  dthetapl=dtheta;  dphipl=dphi ; 
case  2 

xpl=0;  ypl=0;  zpl=0;  Vpl=0;  thetapl=0;  phipl=0; 
dxpl=0;  dypl=0;  dzpl=0;  dVpl=0;  dthetapl=0;  dphipl=0; 
case  3 

xpl=0;  ypl=0;  zpl=10;  Vpl=0;  thetapl=0;  phipl=0; 
dxpl=0;  dypl=0;  dzpl=0;  dVpl=0;  dthetapl=0;  dphipl=0; 
case  0 

xpl=manual  { 1  9 )  ;  ypl=manual  (20  )  ;  zpl=inanual  (21)  ; 

Vpl=inanual  (22 )  ;  thetapl=manual  (23)  ;  phipl=manual  (24)  ; 
dxpl=inanual  (25)  ;  dypl=manual  (2  6)  ;  dzpl=manual  (27 )  ; 
dVpl=inanual  (28)  ;  dthetapl=manual  (29)  ;  dphipl=manual  (30)  ; 
otherwise 

error (' INCORRECT  FILTERl  INTIAL  CONDITIONS') 

end 

switch  f ilter2_scenario 
case  1 

xp2=x;  yp2=y;  zp2=z;  Vp2=V;  thetap2=theta;  phip2=phi; 
dxp2=dx;  dyp2=dy;  dzp2=dz;  dVp2=dV;  dthetap2=dtheta;  dphip2=dphi ; 
case  2 

xp2=0;  yp2=0;  zp2=0;  Vp2=0;  thetap2=0;  phip2=0; 
dxp2=0;  dyp2=0;  dzp2=0;  dVp2=0;  dthetap2=0;  dphip2=0; 
case  3 

xp2=0;  yp2=0;  zp2=10;  Vp2=0;  thetap2=0;  phip2=0; 
dxp2=0;  dyp2=0;  dzp2=0;  dVp2=0;  dthetap2=0;  dphip2=0; 
case  0 

xp2=inanual  ( 4  8  )  ;  yp2=manual  ( 4  9 )  ;  zp2=inanual  (50)  ; 

Vp2=manual (51)  ;  thetap2=manual ( 52 ) ;  phip2=manual (53) ; 
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dxp2=inanual  ( 54  )  ;  dyp2=manual  (55)  ;  dzp2=manual  (56)  ; 
dVp2=inanual  ( 57  )  ;  dthetap2=manual  (58)  ;  dphip2=manual  (5  9)  ; 
otherwise 

error (' INCORRECT  FILTERl  INTIAL  CONDITIONS') 

end 

switch  f ilterl_variance 
case  1 

xpl_v=l;  ypl_v=l;  zpl_v=l;  Vpl_v=.5;  thetapl_v= . I;  phipl_v=.l; 
dxpl_v=.5;  dypl_v=.5;  dzpl_v=.5;  dVpI_v=.I;  dthetapl_v= . 01 ;  .  .  . 
dphipl_v= . 01 ; 

case  0 

xpl_v=manual (32) ;  ypl_v=manual (33) ;  zpl_v=manual(34); 
Vpl_v=manual (35) ;  thetapl_v=manual (36)  ;  phipl_v=manual (37) ; 
dxpl_v=manual ( 38 ) ;  dypl_v=manual (39) ;  dzpl_v=manual(40); 
dVpI_v=manual ( 4 1 ) ;  dthetapl_v=manual ( 42 )  ;  dphipl_v=manual (43) ; 
otherwise 

error (' INCORRECT  FILTERl  STATE  VARIANCES') 

end 

switch  f ilter2_variance 
case  1 

xp2_v=l;  yp2_v=l;  zp2_v=l;  Vp2_v=.5;  thetap2_v= . 1;  phip2_v=.l; 
dxp2_v=.5;  dyp2_v=.5;  dzp2_v=.5;  dVp2_v=.l;  dthetap2_v= . 01 ;  .  .  . 
dphip2_v= . 01 ; 

case  2 

xp2_v=.l;  yp2_v=.l;  zp2_v=.l;  Vp2_v=20;  thetap2_v=4;  phip2_v=4 

dxp2_v=.01;  dyp2_v=.01;  dzp2_v=.01;  dVp2_v=.01;  dthetap2_v= . 01 
dphip2_v= . 01 ; 

case  0 

xp2_v=manual ( 61 ) ;  yp2_v=manual ( 62 ) ;  zp2_v=manual(63); 
Vp2_v=manual ( 64 ) ;  thetap2_v=manual ( 65 )  ;  phip2_v=manual ( 66) ; 
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dxp2_v=manual ( 67 ) ;  dyp2_v=manual { 68 ) ;  dzp2_v=manual{69); 
dVp2_v=manual ( 70 )  ;  dthetap2_v=manual (71)  ;  dphip2_v=manual (72)  ; 
otherwise 

error (' INCORRECT  FILTERl  STATE  VARIANCES') 

end 

switch  f liter l_ineasureinent -Variance 
case  1 

ml  _v=Mu_var  ; 
case  2 

ml_v=. 01; 
case  3 

ml  _v=  .  1 ; 
case  0 

ml_v=manual  (44) ; 
otherwise 

error (' INCORRECT  FILTERl  MEASUREMENT  VARIANCES') 

end 

switch  f liter 2_measurement -Variance 
case  1 

m2  _v=Mu_var  ; 
case  2 

m2_v=. 01; 
case  3 

m2  _v=30 ; 
case  0 

m2_v=manual  (73) ; 
otherwise 

error (' INCORRECT  FILTER2  MEASUREMENT  VARIANCES') 

end 
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switch  f ilterl_weight 
case  1 

W_u=l;  W_v=l;  W_w=l; 
case  0 

W_u=manual  (45)  ;  W_v=manual  ( 4  6 )  ;  W_w=inanual  (47)  ; 
otherwise 

error (' INCORRECT  FILTERl  WEIGHTS') 

end 

switch  f ilter2_weight 
case  1 

W_up=l;  W_vp=l;  W_s=l;  W_up_dot=l;  W_vp_dot=l; 
case  0 

W_up=inanual  (74)  ;  W_vp=manual  (75)  ;  W_s=manual  {16);... 

W_up_dot=manual (77) ;  W_vp_dot=manual (78) ; 
otherwise 

error  (' INCORRECT  FILTER2  WEIGHTS') 

end 


target 

(1) 

=  x;  %x 

target 

(2) 

y;  %y 

target 

(3) 

=  z ;  %  z 

target 

(4) 

V;  % 

V 

target 

(5) 

=  theta 

;  %theta 

target 

(6) 

phi; 

%phi 

target 

(7) 

=  dx; 

%dx 

target 

(8) 

dy; 

%dy 

target 

(9) 

=  dz  ; 

%dz 

target 

(10) 

=  dV; 

%dV 

target 

(11) 

=  dtheta;  %dtheta 

target 

(12) 

=  dphi; 

%dphi 

target 

(13) 

=  xdot; 

%x_dot 
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target  (14) 

=  ydot;  %y_dot 

target (15) 

=  zdot;  %z_dot 

target (16) 

=  TProcess_V ; 

target  { 17 ) 

=  TProcess-theta; 

target (18) 

=  TProcess_phi; 

obs ( 1 )  = 

Mu_var  ; 

obs (2 )  = 

Mv_var  ; 

obs ( 3 )  = 

Mw_var ; 

%EPF-A 

f ilterl ( 1 ) 

=  xpl;  %x 

f ilterl ( 2 ) 

ypl;  %y 

filterl (3) 

=  zpl;  %z 

f ilterl { 4 ) 

Vpl;  %V 

filterl (5) 

=  thetapl;  % 

filterl { 6) 

=  phipl; 

filterl (7) 

=  dxp 1 ; 

filterl { 8 ) 

dypl; 

filterl ( 9) 

=  dzpl; 

filterl (10) 

=  dVp 1 ; 

filterl  (11) 

=  dthetapl; 

filterl (12) 

=  dphipl; 

filterl  (13) 

=  p_num; 

filterl  (14) 

=  xpl_v;  %x  variance 

filterl  (15) 

=  ypl_v;  %y  variance 

filterl (16) 

=  zpl_v;  %z  variance 

filterl  (17) 

=  Vpl_v;  %V  variance 

filterl  (18) 

=  thetapl_v;  %theta 

filterl  (19) 

=  phipl_v;  %phi  vari 
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filterl{20)  = 

dxpl_v;  %dxp  variance 

filterl(21)  = 

dypl_v;  %dYp  variance 

filterl{22)  = 

dzpl_v;  %dzp  variance 

filterl{23)  = 

dVpl_v;  %dVp  variance 

filterl{24)  = 

dthetapl_v;  %dthetap  variance 

filterl{25)  = 

dphipl_v;  %dphip  variance 

filterl{26)  = 

ml_v;  %Measurement  noise  covariance 

filterl{27)  = 

W_u  ; 

filterl{28)  = 

W_v  ; 

filterl{29)  = 

W_w ; 

%EPF-B 


f ilter2 { 1 )  = 

xp2;  %x 

f ilter2 ( 2 )  = 

yp2;  %y 

filter2 (3)  = 

zp2;  %z 

f ilter2 { 4 )  = 

Vp2;  %V 

filter2  (5)  = 

thetap2;  % 

f ilter2  (6)  = 

phip2 ; 

filter2{7) 

dxp2  ; 

f ilter2  (  8 )  = 

dyp2  ; 

f ilter2 (9)  = 

dzp2  ; 

filter2{10)  = 

dVp2  ; 

filter2(ll)  = 

dthetap2 ; 

filter2{12)  = 

dphip2 ; 

filter2{13)  = 

p2_num; 

filter2(14)  = 

xp2_v;  %x  variance 

filter2{15)  = 

yp2_v;  %y  variance 

filter2{16)  = 

zp2_v;  %z  variance 
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filter2 (17) 

Vp2_v;  %V  variance 

filter2  (18) 

= 

thetap2_v;  %theta  variance 

filter2  (19) 

= 

phip2_v;  %phi  variance 

filter2  (20) 

= 

dxp2_v;  %dxp  variance 

filter2 (21) 

= 

dyp2_v;  %dyp  variance 

filter2  (22) 

= 

dzp2_v;  %dzp  variance 

filter2  (23) 

= 

dVp2_v;  %dVp  variance 

filter2  (24) 

= 

dthetap2_v;  %dthetap  variance 

filter2 (25) 

= 

dphip2_v;  %dphip  variance 

filter2  (26) 

= 

m2_v;  %Measurement  noise  covar. 

filter2  (27) 

= 

W_up  ; 

filter2  (28) 

= 

W_vp  ; 

filter2 (29) 

= 

W_s  ; 

filter2 (30) 

= 

W_up_dot ; 

filter2  (31) 

= 

W_vp_dot ; 

err=0;  %Number  of  weight  discrepancy  crashes 
%P re alio cat ion 

%sim_T_G_cent=zeros (params (1) ,12, sim_run) ; 
sim_0_G_cent=zeros  (params  (1)  ,  6,  sim_run)  ; 
s im_P_G_S=zeros (params (1) , 15, sim_run) ; 
sim_Cl_G_cent=zeros (params (1) , 9, sim_run) ; 
sim_P2_G_S=zeros (params (1)  ,  15,  sim_run)  ; 
sim_P2_G_S_part_u=zeros (p2_num, 15, params (1) , sim_run) ; 
s  im_P  2_P_0_part  =  zeros  (p2_num,  5 ,  params  ( 1 )  ,  sim_run )  ; 
sim_P2_P_W=zeros (params (1) , p2_num, sim_run) ; 
sim_C2_G_cent=zeros (params (1) ,9, sim_run) ; 

c=l; 

while  c<=sim_run 
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[T_G_cent,  T_P_cent,  0_G_cent,  P_G_S,  P2_G_S,  P2_G_S_part_u ,  .  .  . 


P2_P_0_part,  P2_P_W,  Cl_G_cent,  C2_G_cent 
EPF_B_Function_Final (params,  target,  obs, 
correction) ; 

%Convert  to  degrees 
if  flag  ==  0 

T_G_cent{:,  [5,6,11,12]) =rad2deg { T_G_cent  ( 
P_G_S(:,  [5, 6, 11, 12]) =rad2deg ( P _G_S  ( :  ,  [5,6 
Cl_G_cent{:,  [8,9])  =rad2deg  (Cl_G_cent(:,  [8 

P2_G_S  (:,  [5,6,11,12]) =rad2deg ( P 2 _G_S  ( :  ,  [5 
C2_G_cent{:,  [8,9])  =rad2deg  (C2_G_cent(:,  [8 

siin_T_G_cent  (:,  :,c)=T_G_cent; 
s iin_0_G_cent  (  : ,  :  ,  c)  =0_G_cent ; 
sim_P_G_S { : , : , c) =P_G_S ; 
sim_Cl_G_cent  (  :  ,  :  ,  c)  =  C  1  _G_cent  ; 
sim_P2_G_S  {  :  ,  : ,  c)  =P2_G_S  ; 

sim_P2_G_S_part_u(:,  :,  :,c)=P2_G_S_part_u; 
sim_P2_P_0_part  (  :  ,  :  ,  :  ,  c)  =P2_P_0_part ; 
sim_P2_P_W  {  :  ,  :,c)=P2_P_W; 
s  im_C2  _G_cent  (  :  ,  :  ,  c)  =  C2  _G_cent  ; 
c=c+l ; 

end 

if  flag  ==  1; 

message= 'WEIGHT  DISCREPANCY'; 
err=err+l ; 

end 

c 

err 


flag]  = . . . 
filterl,  filter2,. 


:,  [5,6,11,12])); 
11,12] ) ) ; 

9]  )  )  ; 

6, 11, 12] )  )  ; 

9]  )  )  ; 
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end 


err 

switch  sim_plot 
case  1 

fig=l; 

hfig=figure (fig) ; 

set (hfig,  ' Position ',[  0 ,  0,  800,  800]);  %  [x  y  width  height] 
axis  equal 
xlabel ( ' X ' ) ; 
ylabel ( ' Y ' ) ; 
z label ( ' Z ' ) ; 
for  i2=4 : params ( 1 ) 
hold  on; 

scatters (T_G_cent (12, 1) , T_G_cent  (i2, 2) , T_G_cent  (12, 3) ,  'b' ) ; 
scatters  (0_G_cent  (12, 4) ,  0_G_cent  (i2, 5) ,  0_G_cent  (12, 6) ,  'g' ) ; 
scatters (Cl_G_cent (i2, 1) , Cl_G_cent (i2, 2) , Cl_G_cent (i2, 3) , 'm' ) ; 
scatters (P_G_S  (12,1), P_G_S  (12,2), P_G_S  (12,3),  ' r ' )  ; 
scatters (P2_G_S  (12,1)  ,  P2_G_S (12,2),  P2_G_S  (12,3),  ' r+ ' )  ; 
scatters (C2_G_cent (12, 1) , C2_G_cent (12, 2) , C2_G_cent (12, 3) , 'm+' ) ; 
pause  (  .  1 ) 

end 

legend ( ' Target ' , ' Measurement ' , ' Comparison ' , 'Filter ' , . . . 

' Location ' , ' East ' ) 
fig=f ig+1; 

ang_red=@ (a)  atan2 (sin (a) , cos (a) ) ; 

for  1=4 : params ( 1 ) 

figure (fig) ; clf; 

set (gcf,  ' position ',  [  680,  72,  1152,  906]); 

%  Plot  weighted  particle  distribution  for  diagnostic  pusposes 
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a=subplot  (3,2,1); 

stem3  {P2_P_0_part  {  :  ,  1,  i)  ,  P2_P_0_part  (  :  ,  2 ,  i )  ,  P  2  _P _W  ( i ,  :  )  ,  '  ro  '  )  ; 
hold  on;  stein3  (T_P_cent  (i,  1)  ,  T_P_cent  (i,  2)  ,  .  .  . 

max ( P  2  _P  _W ( 1 ,  : ) ) ,  ' k* ' ) ; 
title (a, 'U  vs  V  Position  Particle  Weight'); 

daspect ( [lei, lei, le-2] ) ; drawnow; 
a=subplot  (3,2,3) ; 

stem  (P2_P_0_part  (  :  ,  3,  i)  ,  P2_P_W  (i,  :  )  ,  '  ro  '  )  ; 

hold  on;  stem ( T_P_cent (i, 3 ) , max ( P 2 _P_W ( i , : ) ) , ' k* ' ) ; 

title (a, 'Z  Position  Particle  Weight'); 

daspect ( [ le-4 , le-2 , 1 ] ) ; drawnow; 
a=subplot (3,2,5) ; 

stem3  (P2_P_0_part  (  :  ,  4,  i)  ,  P2_P_0_part  (  :  ,  5 ,  i )  ,  P  2  _P _W  ( i ,  :  )  ,  '  ro  '  )  ; 
hold  on;  stem3  (T_P_cent  (i, 4) , T_P_cent  (i, 5) ,  .  .  . 

max ( P  2  _P  _W ( i ,  : ) ) ,  ' k* ' ) ; 
title (a, 'U  vs  V  Velocity  Particle  Weight'); 
daspect ( [le2, le2, le-2] ) ; drawnow; 

a=subplot  (3,2,2) ; 

stem3  (P2_G_S_part_u(:,l,i),P2_G_S_part_u(:,2,i),  .  .  . 

P2_P_W (i, : ) , ' ro ' ) ; 

hold  on;  stem3 ( T_G_cent  ( i , 1 ) , T_G_cent ( i , 2 ) ,  .  .  . 

max ( P  2  _P  _W ( i,  : ) ) ,  ' k* ' ) ; 
title (a, 'X  vs  Y  Position  Particle  Weight'); 

daspect ( [lei, lei, le-2] ) ; drawnow; 
a=subplot  (3,2,4); 

stem3  (P2_G_S_part_u(:,3,i),P2_G_S_part_u(:,4,i)  ,  .  .  . 

P2_P_W (i, : ) , ' ro ' ) ; 

hold  on;  stem3 (T_G_cent  (i, 3) , T_G_cent (i, 4) ,  .  .  . 

max ( P  2  _P  _W ( i ,  : ) ) ,  ' k* ' ) ; 
title (a, 'Z  Position  Particle  Weight'); 
daspect ( [lei, lei, le-2] ) ; drawnow; 
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a=subplot (3,2,6); 

stem3  {ang_red(P2_G_S_part_u{:,5,i)  ),  .  .  . 

ang_red  (P2_G_S_part_u  (  :  ,  6,  i)  )  ,  P2_P_W  (i,  :  )  ,  '  ro  '  )  ; 
hold  on;  stein3  (ang.red  (T_G_cent  (i,  3)  )  ,  .  .  . 

ang_red(T_G_cent  (1,4)  ) , max ( P  2  _P_W ( 1 ,  : ) ) ,  ' k*  '  ) ; 
title (a,  'X  vs  Y  Velocity  Particle  Weight'); 
pause ( ) ; 

end 


for  1=4 : params ( 1 ) 

%Weight  Scatter  Plots 
figure (fig) ; clf; 

set (gcf,  ' position ',[  0 ,  0,  800,  1200]); 
a=subplot  (3,2,1); 

stem3  (P2_P_0_part  (  :  ,  1, 1)  ,  P2_P_0_part  (  :  ,  2, 1)  ,  .  .  . 
P2_P_W (1, : ) , ' ro ' ) ; 

hold  on;  stem3 (T_P_cent  (1, 1) , T_P_cent (1, 2) ,  .  .  . 

max ( P  2  _P  _W ( 1 ,  : ) ) ,  ' k* ' ) ; 
title (a, 'U  vs  V  Position  Particle  Weight'); 
xlabel (a,  ' u ' ) ; y label  (a,  ' v ' ) ; 

a=subplot  (3,2,3); 

stem (P2_P_0_part ( : , 3, 1) , P2_P_W (i, : ) , ' ro ' ) ; 

hold  on;  stem ( T_P_cent (1, 3 ) , max ( P 2 _P_W ( 1 , ; ) ) , ' k* ' ) ; 

title (a, 'W  Position  Particle  Weight'); 

xlabel (a, ' looming ' ) ; 

a=subplot (3,2,5)  ; 

stem3  (P2_P_0_part  (  :  ,  4, 1)  ,  P2_P_0_part  (  :  ,  5, 1)  ,  .  .  . 
P2_P_W (1, : ) , ' ro ' ) ; 

hold  on;  stem3 (T_P_cent  (1, 4) , T_P_cent (1, 5) ,  .  .  . 
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max ( P  2  _P  _W ( i ,  : ) ) ,  ' k* ' ) ; 


title (a, 'U  vs  V  Velocity  Particle  Weight'); 
xlabel (a,  'u_d_o_t');  y label (a,  'v_d_o_t'); 

a=subplot (3,2,2)  ; 

temp_r=T_G_cent { 1 , 1:3)  .  ' /norm ( T_G_cent { 1 , 1:3) ) ; 
stem (P2_G_S_p art _u (:, 1:3,1) *temp_r ,  P2_P_W  (1,  : )  ,  ' ro ' ) ; 
hold  on;  stem  (T_G_cent  (1,1:3) *temp_r  ,  max ( P  2  _P_W ( 1 ,  : ) )  ,  'k*'); 
title  (a,  ' Position  Particle  Weight'); 
xlabel (a, ' r ' ) ; 

a=subplot  (3,2,4); 

stem  (P2_G_S_part_u  (  :  ,  4,  i)  ,  P2_P_W  (i,  :  )  ,  '  ro  '  )  ; 
hold  on;  stem ( T_G_cent (i, 4 ) , max ( P 2 _P_W ( i , : ) ) , ' k* ' ) ; 
title (a, 'Velocity  Particle  Weight'); 
xlabel  (a,  'V  )  ; 

a=subplot (3,2,6); 

stem3 (ang_red(P2_G_S_part_u ( :,5,i) ) , . . . 

ang.red  (P2_G_S_part_u  (  :  ,  6,  i)  )  ,  P2_P_W  (i,  :  )  ,  '  ro  '  )  ; 
hold  on;  stem3 (ang_red (T_G_cent  (i, 5)  ) ,  .  .  . 

ang_red(T_G_cent  (i, 6)  ) , max ( P  2  _P_W ( i ,:)),' k* ') ; 
title (a,  'X  vs  Y  Velocity  Particle  Weight'); 
xlabel (a, ' \theta ' ) ; ylabel (a, ' \phi ' ) ; 
pause ( . 1 ) ; 

end 

%Comparison  of  States 
time=zeros (params (1)  ,  1)  ; 
for  i2=3 : params ( 1 ) 
time (12 , 1 ) =i2 ; 


end 
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%Positions 


fig=f ig+1; 
hfig=figure (fig) ; 

set (hfig,  ' Posit ion ',[  0 ,  0,  800,  1200]);  %  [x  y  width  height] 

a=subplot  (3,1,1); 
plot  (time,  T_G_cent  (  :  , 1 ) ,  '  b ' ,  .  .  . 
time, P_G_S  (:,1),  'r',  .  .  . 
time, P2_G_S(:,l),'r:',... 
time, Cl_G_cent  (:,1) ,  'g',  .  .  . 
time, C2_G_cent  (:,1)  ,  'g: ') ; 

legend ( ' Target ', 'Filter  1', 'Filter  2',' Comparison  1 ' , . . . 

' Comparison  2 ' ,  ' Location ' ,  ' SouthEast '  ) 
title (a, 'X  Position'); 

b=subplot (3,1,2); 
plot  (time,  T_G_cent  (  :  , 2 ) ,  ' b ' ,  .  .  . 
time, P_G_S  (:,2),  'r',  .  .  . 
time, P2_G_S(:,2),'r:',... 
time, Cl _G -Cent  (:,2) ,  'g',  .  .  . 
time, C2_G_cent(:,2),  'g:'); 

legend ( ' Target ', 'Filter  1', 'Filter  2',' Comparison  1 ' , . . . 

' Comparison  2 ' ,  ' Location ' ,  ' SouthEast ' ) 
title  (b,'Y  Position'); 

c=subplot (3, 1,3); 
plot  (time,  T_G_cent  (  :  , 3) ,  'b ' ,  .  .  . 
time, P_G_S  (:,3),  'r',  .  .  . 
time, P2_G_S(:,3),'r;',... 
time, Cl _G -Cent  (:,3) ,  'g',  .  .  . 
time, C2_G_cent  (:,3) ,  'g:  ') ; 

legend ( ' Target ',  'Filter  1',  'Filter  2',' Comparison  1 ' ,  . .  . 
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' Comparison  2 ' , ' Location ' , ' SouthEast ' ) 
title  (c,'Z  Position'); 
fig=f ig+1; 

%Velocity  Magintude  and  headings 
hfig=figure (fig) ; 

set (hfig,  ' Position ',[  0 ,  0,  800,  1200]);  %  [x  y  width  height] 

a=subplot  (3,1,1); 
plot  (time,  T_G_cent  (  :  , 4 ) ,  '  b ' ,  .  .  . 
time, P_G_S  (:,4),  'r',  .  .  . 
time, P2_G_S(:,4),'r:',... 
time, Cl_G_cent  (:,7)  ,  'g',  .  .  . 
time, C2_G_cent  (:,7) ,  'g:  ') ; 

legend ( ' Target ', 'Filter  1', 'Filter  2',' Comparison  1 ' , . . . 

' Comparison  2 ' ,  ' Location '  ,  ' SouthEast ' ) 
title  (a,  'Velocity  Magnitude ' ) ; 

b=subplot (3,1,2); 

plot (time,  (  T_G_cent  (  :  , 5 ) ) ,  ' b ' ,  ... 
time, (P_G_S(:,5)),'r',... 
time, (P2_G_S(:,5)),'r:',... 
time,  (Cl_G_cent(:,8)),  'g',  ... 
time,  ( C2  _G_cent ( : , 8 ) ) ,  ' g :  ' ) ; 

legend ( ' Target ',  'Filter  1',  'Filter  2',' Comparison  1 ' ,  . .  . 

' Comparison  2 ' ,  ' Location  '  ,  ' SouthEast '  ) 
title (b, ' Theta  Heading'); 

c=subplot (3,1,3); 

plot (time,  (  T_G_cent  (  :  , 6 ) ) ,  ' b ' ,  ... 
time, (P_G_S(:,6)),'r',... 
time, (P2_G_S(:,6)),'r:',... 
time,  (Cl_G_cent(:,9)),  'g',  .  .  . 
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time,  { C2  _G_cent  {  :  , 9 ) ) ,  '  g :  '  )  ; 

legend ( ' Target 'Filter  1', 'Filter  2',' Comparison  1 ' , . . . 

' Comparison  2 ' , ' Location ' , ' SouthEast ' ) 
title (c, 'Phi  Heading' ) ; 
fig=f ig+1; 


otherwise 

message='WILL  NOT  PLOT,  MULTIPLE  SIMULATION  RUNS'; 
err 

Calculate  performace  abilities 

%%NOTE:  Although  distance  and  V  errors  are  absolute,  heading  errors 
%can  only  be  up  to  180  degrees  off  for  both  phi  and  theta  (ie.  you 
%are  pointinig  in  the  complete  opposite  direction.  Theta  is 
%already  between  0  and  180,  so  aboslute  errors  may  be  taken.  Phi 
%though  must  be  adjusted  so  that  all  errors  are  between  0  and  180 
%(ex.  an  'error'  of  350  is  really  only  an  error  of  10 

%Preallocate 

er ror_m_f =zeros (params (1) , 4) ; 
er ror_m_c=zeros (params (1) , 4) ; 
vec_sim_P_G_S=zeros (params (1) , 4) ; 
vec_s  im_Cl_G_cent  =  zeros  (params  (1)  ,4)  ; 

%'Best'  Values  for  filter  and  comparison 
range_index=solution_range*  s im_run ; 
for  i=2 : params ( 1 ) 

for  i2=l:sim_run 

%Angle  Adjustment 
phi=sim_P2_G_S (i, 6,  i2)  ; 
if  (phi  >  360) 
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mult=floor (phi/  (360) ) ; 
phi=phi— mult*360 ; 

end 

if  (phi  <  -360) 

mult=floor(phi/ (-360) ) ; 
phi=phi+mult*360 ; 

end 

if  (phi  <  0) 

phi=360+phi; 

end 

sim_P2_G_S (i, 6,i2) =abs (phi) ; 

theta=siin_P2_G_S  (i,  5,  i2)  ; 

theta=wrapTo2Pi (theta) ; 

if (theta>pi ( ) ) 

theta_a=theta-pi ()  ; 
theta=pi ( ) -theta.a; 

end 

sim_P2_G_S (i,5,i2) =theta; 

%Create  variable  vectors 

t_dist=sqrt ( s im_T_G_cent (i,l,i2) "2+ . . . 

siin_T_G_cent  (i,2,i2)  ''2  +  sim_T_G_cent  (i,2,i2)  *2)  ; 

f_dist=sqrt  (sim_P_G_S  (i,l,i2)  ''2+sim_P_G_S  (i,2,i2)  "2  + 
sim_P_G_S  (i,2,i2)''2); 

c_dist=sqrt  (  sim_Cl_G_cent  (i,l,i2)  ''2+.  .  . 

s im_C  1  _G_cent  (i,  2, 12)  ''2  +  sim_Cl_G_cent  (i,  2,  i2)  "2) 

f2_dist=sqrt  (  sim_P2_G_S  (i,l,i2)''2+... 

sim_P2_G_S  (i,  2,  i2)  2  + s  im_P 2 _G_S  ( i ,  2 ,  i2  )  '' 2  )  ; 
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c2_dist=sqrt  { sim_C2_G_cent  (i,l,i2)  ''2+.  .  . 

sim_C2_G_cent  (i,  2,  i2)  ''2  +  sim_C2_G_cent  (i,2,i2)  "2)  ; 

vec_siin_P_G_S  {i2, 1)  =abs  {f_dist-t_dist)  ; 

vec_siin_P_G_S{i2,  [2,3,4])  =abs  (sim_P_G_S(i,  [4, 5, 6], 12)-. 

sim_T_G_cent (i, [4,5, 6] ,12) ) ; 
vec_slm_Cl_G_cent (12, 1) =abs (c_dlst-t_dlst ) ; 
vec_slm_Cl_G_cent  (12,  [2,3,4]  )=.  .  . 

abs  (slin_Cl_G_cent  (1,  [7,  8,  9]  )-.  .  . 
slm_T_G_cent (1, [4,5, 6] ,12) ) ; 

vec_slm_P2_G_S (12,1) =abs (f2_dlst-t_dlst) ; 
vec_slm_P2_G_S(12,  [2,3,4])  =abs  (slm_P2_G_S(l,  [4, 5, 6], 12) 
slm_T_G_cent (1, [4,5, 6] ,12) ) ; 
vec_slm_C2_G_cent (12, 1) =abs (c2_dlst-t_dlst ) ; 
vec_slm_C2_G_cent  (12,  [2,3,4]  )=.  .  . 

abs(slin_C2_G_cent  (1,  [7, 8, 9], 12)-.  .  . 
slm_T_G_cent (1, [4,5, 6] ,12) ) ; 

%Phl  adjustment 
If  vec_slm_P_G_S  (12,  4)  >180 

vec_slm_P_G_S  (12,4)=3  60— vec_slm_P_G_S  (12,4)  ; 

end 

If  vec_slm_Cl_G_cent  (12, 4)  >180 

vec_slm_Cl_G_cent  (12, 4)  =360— vec_slm_Cl_G_cent  (12, 4) 

end 

If  vec_slm_P2_G_S  (12,  4)  >180 

vec_slm_P2_G_S  (12, 4)  =360— vec_slm_P2_G_S  (12,4)  ; 

end 
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if  vec_sim_C2_G_cent  (i2, 4)  >180 

vec_sim_C2_G_cent  (12, 4) =360— vec_sim_C2_G_cent  (i2, 4) 

end 

end 

svec_sim_P_G_S=sort  (vec_sim_P_G_S  ,  1)  ; 
svec_sim_Cl_G_cent=sort  (vec_siin_Cl_G_cent  ,  1)  ; 
error_b_f  (i,  :  )  =svec_s  im_P_G_S  ( range_index ,  :  )  ; 
error_b_c  (i,  :  )  =svec_sim_Cl_G_cent  (range_index,  :  )  ; 

svec_sim_P2_G_S=sort  (vec_siin_P2_G_S  ,  1)  ; 
svec_sim_C2_G_cent=sort  (vec_siin_C2_G_cent  ,  1)  ; 
error _b_f2  (i,  :)=svec_sim_P2_G_S  (range .index,  :)  ; 
error_b_c2  (i,  :)  =svec_sim_C2_G_cent  ( range.index,  :  )  ; 

end 

%Mean  or  average  error  values  for  filter  and  comparison 
for  i=2 : params ( 1 ) 

for  i2=l:sim_run 

%Create  variable  vectors 

t_dist=sqrt  (  sim_T_G_cent  (i,l,i2)  "'2+.  .  . 

sim_T_G_cent  (i,2,i2)  ''2  +  sim_T_G_cent  (i,3,i2)  '2)  ; 
f _dist=sqrt  (sim_P_G_S  (i,l,i2)  ''2+sim_P_G_S  (i,2,i2)  "'2+.  .  . 

sim_P_G_S  (i,3,i2)''2); 
c_dist=sqrt ( sim_Cl_G_cent  (i,l,i2)  "2+.  .  . 

s im_C  1  _G_cent  (i,  2, 12)  ''2  +  sim_Cl_G_cent  (i,3,i2)  "2)  ; 

f2_dist=sqrt  (  sim_P2_G_S  (i,  1,  i2)  " 2+ s im_P 2 _G_S  (i,  2,  i2)  "'2  + 
sim_P2_G_S  (i,3,i2)''2); 
c2_dist=sqrt  ( sim_C2_G_cent  (i,l,i2)  ''2+.  .  . 

sim_C2_G_cent  (i,  2, 12)  ''2  +  sim_C2_G_cent  (i,3,i2)  "2)  ; 
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vec_siin_P_G_S  {i2, 1)  =abs  {f_dist-t_dist)  ; 

vec_siin_P_G_S{i2,  [2,3,4])  =abs  (sim_P_G_S(i,  [4,5,6],i2)-. 
siin_T_G_cent  (i,  [4,5,  6]  ,12)  )  ; 

vec_sim_Cl_G_cent (12, 1) =abs (c_dlst-t_dlst ) ; 

vec_slm_Cl_G_cent  (12,  [2,3,4]  )=.  .  . 

abs(slin_Cl_G_cent  (1,  [7, 8, 9], 12)-.  .  . 
slin_T_G_cent  (1,  [4,5,  6]  ,12)  )  ; 

vec_slm_P2_G_S (12,1) =abs (f2_dlst— t_dlst) ; 

vec_slm_P2_G_S (12,  [2,  3,  4] ) =abs (slm_P2_G_S(l,  [4, 5, 6], 12) 
slin_T_G_cent  (1,  [4,5,  6]  ,12)  )  ; 

vec_slm_C2_G_cent (12, 1) =abs (c2_dlst-t_dlst ) ; 

vec_slm_C2_G_cent  (12,  [2,3,4]  )=.  .  . 

abs(slin_C2_G_cent  (1,  [7, 8, 9], 12)-.  .  . 
slm_T_G_cent (1, [4,5, 6] ,12) ) ; 

If  vec_slm_P_G_S  (12,  4)  >180 

vec_slm_P_G_S  (12,4)=360— vec_slm_P_G_S  (12,4)  ; 

end 

If  vec_slm_Cl_G_cent  (12, 4)  >180 

vec_slm_Cl_G_cent  (12, 4)  =360— vec_slm_Cl_G_cent  (12, 4) 

end 

If  vec_slm_P2_G_S  (12,  4)  >180 

vec_slm_P2_G_S  (12, 4)  =360— vec_slin_P2_G_S  (12,4)  ; 

end 

If  vec_slm_C2_G_cent  (12, 4)  >180 

vec_slm_C2_G_cent  (12, 4) =360— vec_slm_C2_G_cent  (12, 4) 

end 

end 


231 


error_m_f  (i,  1)  =mean  (vec_sim_P_G_S  (  :  ,  1)  )  ; 


error_m_f  (i,  2)  =mean  (vec_sim_P_G_S  (  :  ,  2)  )  ; 
error_m_f  (i,  3)  =mean  (vec_sim_P_G_S  (  :  ,  3)  )  ; 
error_m_f  (i,  4)  =mean  (vec_sim_P_G_S  (  :  ,  4 )  )  ; 

error_m_c (i,  1) =mean (vec_sim_Cl_G_cent {:,!)); 
error_m_c  (i,  2)  =mean (vec_sim_Cl_G_cent  {  :  , 2) )  ; 
error_m_c  (i,  3)  =mean  (vec_sim_Cl_G_cent  {  :  ,  3)  )  ; 
error_m_c  (i,  4)  =mean  (vec_sim_Cl_G_cent  {  :  ,  4)  )  ; 

error_m_f2  (i,  1)  =mean  (vec_sim_P2_G_S  (:,!)); 
error_m_f2  (i,  2)  =mean  (vec_sim_P2_G_S  (:,2)); 
error_m_f2  (i,  3) =mean (vec_sim_P2_G_S  ( : , 3) ) ; 
er  ror_m_f  2  ( i,  4  )  =mean  ( vec_s  im_P  2  _G_S  (  :  ,  4  )  )  ; 

error_m_c2  (i,  1)  =mean  (vec_siin_C2_G_cent  {  :  ,  1)  )  ; 
error_m_c2  (i,  2) =mean (vec_sim_C2_G_cent  {: ,2) ) ; 
error_m_c2  (i,  3) =mean (vec_sim_C2_G_cent { : , 3) ) ; 
err or_m_c2  ( i,  4  )  =mean  (vec_siin_C2_G_cent  {  :  ,  4)  )  ; 

end 

time=zeros (params (1) , 1) ; 
for  12=2 : params ( 1 ) 
time (12 , 1 ) =12 ; 

end 

fig=l; 

%TER 

hfig=figure (fig) ; 

set (hfig,  ' Position ',[ 0 ,  0,  800,  1200]);  % [x  y  width  height] 

a=subplot (4,1,1); 
plot (time, error_b_f ( : ,  1 )  ,  ' m '  ,  .  .  . 
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time, error _b_f2  'r:  .  .  . 

time, error _b_c(:,l),  'g',  .  .  . 
time,  error_b_c2  (  : , 1 ) ,  ' g :  ' ) ; 

legend {' Filter  1', 'Filter  2 Comparison  1 Comparison  2', 
'Target', 'Location', 'North', 'Orientation', 'horizontal') 
title (a, ' Distance  Error'); 
xlabel  (a,  ' Time  Step'); 
ylabel (a, ' Error ' ) ; 

b=subplot  (4,1,2); 
plot (time, error _b_f (:,2),  'm',  .  .  . 
time, error _b_f2  (:,2),  'r:  ',  .  .  . 
time, error_b_c(:,2),  'g',  ... 
time, error_b_c2  (:,2),  'g:  ',  .  .  . 
time, T_G_cent  (  : , 4 ) ,  ' b  '  ) ; 

legend ( ' Filter  1 ' , ' Filter  2 ' , ' Comparison  1 ' , ' Comparison  2 ' , 
'Target',  'Location',  'North',  'Orientation',  'horizontal') 
title (b, 'Velocity  Error'); 
xlabel (b, ' Time  Step ' ) ; 
ylabel (b, 'Error' ) ; 

c=subplot (4,1,3); 
plot (time, error_b_f ( : ,  3 )  ,  ' m '  ,  .  .  . 
time, error _b_f2  (:,3),  'r:  ',  .  .  . 
time, error_b_c(:,3),  'g',  .  .  . 
time,  error_b_c2  (:,3),  'g:  ',  .  .  . 
time, T_G_cent ( : , 5 ) , 'b ' ) ; 

legend ( ' Filter  1 ' ,  ' Filter  2 ' ,  ' Comparison  1 '  ,  ' Comparison  2 '  , 
'Target', 'Location', 'North', 'Orientation', 'horizontal') 
title (c, ' Theta  Heading  Error'); 
xlabel (c, ' Time  Step'); 
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ylabel (c,  'Error  (Deg)  ' )  ; 


d=subplot (4,1,4); 
plot (time, error_b_f ( : ,  4 )  ,  '  m '  ,  .  .  . 
time, error _b_f2  (:,4),  'r:  ',  .  .  . 
time,  error_b_c(:,4),  'g',  .  .  . 
time, error_b_c2 (:,4) , 'g: ', . . . 
time, T_G_cent  (  : , 6 ) ,  'b '  ) ; 
legend (' Filter  1', 'Filter  2 ',' Comparison 
'Target', 'Location', 'North', 'Orientat 
title (d, 'Phi  Heading  Error'); 
xlabel (d, ' Time  Step'); 
ylabel (d,  'Error  (Deg)  '  )  ; 
f ig=f ig+1 ; 


%MAE 

hfig=figure (fig) ; 

set(hfig, 'Posit ion', [0,  0,  800,  1200]);  %[xy 
a=subplot (4,1,1); 

plot (time, error_m_f ( :  ,  1)  ,  'm'  ,  .  .  . 
time, er ror_m_f 2(:,1),  'r:',  ... 
time,  error_m_c ( :  ,  1 )  ,  ' g '  ,  .  .  . 
time,  error_m_c2  (  : , 1 ) ,  ' g :  ' ) ; 
legend (' Filter  1', 'Filter  2 ',' Comparison 
'Target', 'Location', 'North', 'Orientat 
title (a, ' Distance  Error'); 
xlabel (a, ' Time  Step'); 
ylabel  (a,  ' Error ' ) ; 

b=subplot  (4,1,2); 

plot (time, error_m_f (  : ,  2 )  ,  '  m '  ,  .  .  . 
time, er ror_m_f  2(:,2),'r:',... 


1 ' ,  ' Comparison  2 '  , 
ion', 'horizontal') 


width  height] 


1 ' ,  ' Comparison  2 '  , 
ion', 'horizontal') 
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time,  error_m_c ( :  ,  2 )  ,  ' g '  ,  .  .  . 
time,  error_m_c2  ( : , 2 ) ,  ' g :  ' ,  .  .  . 
time, T_G_cent  {  : , 4 ) ,  ' b ' ) ; 

legend {' Filter  1', 'Filter  2 Comparison  1 Comparison  2', 
'Target', 'Location', 'North', 'Orientation', 'horizontal') 
title (b, 'Velocity  Error'); 
xlabel (b, ' Time  Step ' ) ; 
ylabel (b, 'Error' ) ; 

c=subplot  (4,1,3); 

plot (time, error_m_f (  : ,  3 )  ,  ' m '  ,  .  .  . 
time,  error_m_f  2  (  : ,  3 )  ,  '  r :  '  ,  .  .  . 
time,  er ror_m_c (  :  ,  3 )  ,  ' g  '  ,  .  .  . 
time, er ror_m_c2  ( : ,  3 )  ,  ' g :  '  ,  .  .  . 
time, T_G_cent ( : , 5 ) , 'b ' ) ; 

legend ( ' Filter  1 ' ,  ' Filter  2 '  ,  ' Comparison  1 '  ,  ' Comparison  2 ' , 
'Target', 'Location', 'North', 'Orientation', 'horizontal') 
title  (c,  ' Theta  Heading  Error'); 
xlabel (c, ' Time  Step'); 
ylabel (c, 'Error  (Deg) ' ) ; 

d=subplot (4,1,4); 

plot (time, error_m_f ( : ,  4 )  ,  '  m '  ,  .  .  . 
time, er ror_m_f 2(:,4),  'r:',  ... 
time, error_m_c ( :  ,  4 )  ,  ' g '  ,  .  .  . 
time,  error_m_c2  ( :  ,  4 )  ,  ' g :  '  ,  .  .  . 
time, T_G_cent ( : , 6 ) , 'b ' ) ; 

legend (' Filter  1', 'Filter  2 ',' Comparison  1 ',' Comparison  2', 
'Target', 'Location', 'North', 'Orientation', 'horizontal') 
title (d, 'Phi  Heading  Error'); 
xlabel (d, ' Time  Step'); 
ylabel (d,  'Error  (Deg)  '  )  ; 
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f ig=f ig+1 ; 


%Final  50  step  metrics 
best_error_c=mean (error_t 
best_error_c=mean (error_t 
best_error_f =mean (error_t 
best_error_f =mean (error_t 

%Final  50  step  metrics 
mean_error_c=mean (error 
mean_error_c=mean (error 
mean_error_f =mean (error 
mean_error_f =mean (error 

end 


.c  (params  ( 1 )  -  50  :  params  ( 1 )  ,  :  )  ) 
c2  (params ( 1 ) -50 : params ( 1 )  ,  : ) ) 
.f  (params  ( 1 )  -  50  :  params  ( 1 )  ,  :  )  ) 
f 2 (params (1) -50 : params (1) , : ) ) 

.m_c (params ( 1 ) -50 : params ( 1 ) , : ) ) 
.m_c2  (params  ( 1 )  -  50  :  params  ( 1 )  ,  :  )  ) 
.m_f (params ( 1 ) -50 : params ( 1 ) , : ) ) 
.m_f 2 (params (1) -50 :params (1) , : ) ) 


A.5.2  EPF  JB_F  unction  _Final. 

function  [T_G_cent,  T_P_cent,  0_G_cent,  P_G_S,  P2_G_S,  P2_G_S_part_u ,  .  .  . 
P2_P_0_part,  P2_P_W,  Ll_G_cent,  L2_G_cent,  flag]  =... 

EPF_B_Funct ion_Final (params ,  target,  obs,  filterl,  filter2,  correction) 
%%  This  function  is  Particle  Filter  A,  allowing  for  multiple  scenarios  or 
%%  simulation  runs.  Inputs  are  contained  within  a  separate  .mfile  that 
%%  executes  this  file. 

%%  Initial  conditions  and  setup 

rng (' shuffle ' )  %Shuffle  random  numbers 

%System  Parameters 

T  =  params (1);  %Number  of  iterations 
dt  =  params (2);  %Time  step 

f  =  params (3);  %Focal  Length 

f lag=0 ; 
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correct=correction; 


%Rotation  matrix  between  global  and  camera  frames 
Rot_y=@ (a)  [cos (a) ,  0,  -sin{a);  0,1,0;  sin(a),  0,  cos (a) ]  ; 

Rot_x=@ (a)  [1, 0,  0;  0,  cos (a)  ,  -sin(a);  0,sin(a),  cos (a) ] ; 

Rot_z=@ (a)  [cos (a) , -sin (a) , 0; sin (a) ,  cos{a),0;0,0,l]; 

PlotDCM=@ (A, 0)  plots (cumsum {[0(1),A(1,1)]), cumsum {[0(2),A(2,1)]),... 
cumsum ([0(3),A{3,1)]), 'r-', . . . 

cumsum { [0(1) ,A(1,2) ] ) , cumsum { [0(2) ,A(2,2) ] ) , cumsum ([0(3),A(3,2)]), 'g-', 
cumsum ( [0(1) ,A(1,3) ] ) , cumsum ( [0(2) ,A(2,3) ] ) , cumsum ([0(3),A(3,3)]), 'b-', 
' linewidth ' , 5 ) ; 

ang_red=@ (a)  atan2 (sin (a) , cos (a) ) ; 

%%  Target  Parameters 


%Truth 

conditions,  the  inital  conditions  of  the  centroid 

X  = 

target ( 1 )  ; 

Y 

target (2 ) ; 

z  = 

target ( 3 ) ; 

V 

=  target ( 4 ) ; 

theta 

=  deg2rad (target ( 5 ))  ; 

phi 

=  deg2rad (target ( 6) )  ; 

dx 

target ( 7 ) ; 

dy  = 

target ( 8 ) ; 

dz  = 

target ( 9) ; 

dV 

=  target (10); 

dtheta 

=  deg2rad (target ( 1 1 ))  ; 

dphi 

=  deg2rad (target ( 12 ))  ; 

xdot 

=  target (13) ; 

ydot 

=  target ( 14 ) ; 

zdot 

=  target (15) ; 

T_G_cent(l, :)=[x  y  z  V  theta  phi  dx  dy  dz  dV  dtheta  dphi  xdot  ydot  zdot]; 
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%Process  Noise 


TS_V (1) =target (16); 
TS_V (2) =target (17); 
TS_V (3) =target (18)  ; 


%P re alio cat ion 
T_C_cent=zeros (T, 12) ; 
T_G_Rotdata=zeros (T, 3) ; 
T_RotG2C=zeros (3, 3, T)  ; 
T_RotC2G=zeros (3, 3,  T)  ; 
T_P_cent  =  zeros (T,  5)  ; 


%%  Measurement  conditions 
%Initial  point  locations 
0_G_point_a=  [  T_G_cent  (1)- 
0_G_point_b=  [  T_G_cent  (1)  + 
0_G_point_c=  [  T_G_cent  (1)- 
0_G_point_d= [ T_G_cent  (1)- 
0_G_point_e=  [  T_G_cent  (1)- 
0_G_point_f = [ T_G_cent  (1)  + 
0_G_point_g=  [  T_G_cent  (1)  + 
0_G_point_h=  [  T_G_cent  (1)  + 


(for  a  cube),  based  of  T  centroid 


.  5  T_G_cent (2 ) - . 5 
.  5  T_G_cent  (2 ) - . 5 
.  5  T_G_cent  (2)  +  .  5 
.  5  T_G_cent (2 ) - . 5 
.  5  T_G_cent  (2)  +  .  5 
.  5  T_G_cent (2 ) - . 5 
.  5  T_G_cent  (2)  +  .  5 
.  5  T_G_cent  (2)  +  .  5 


T_G_cent ( 3 ) - . 5 ] ; 
T_G_cent  ( 3 ) -  .  5 ] ; 
T_G_cent  (3) - . 5] ; 
T_G_cent  ( 3 ) + . 5 ] ; 
T_G_cent ( 3 ) + . 5 ] ; 
T_G_cent ( 3 ) + . 5 ]  ; 
T_G_cent ( 3 ) - . 5 ]  ; 
T_G_cent ( 3 ) +  .  5 ]  ; 


%Form  the  initial  cube  based  of  points 

0_G_points=  [0_G_point_a;  0_G_point_b;  0_G_point_f ;  0_G_point_h;  .  .  . 
0_G_point_g;  0_G_point_c;  0_G_point_a;  0_G_point_d;  .  .  . 
0_G_point  _e  ;  0_G_point  _h ;  0_G_point  _f  ;  0_G_point  _d;  .  .  . 
0_G_point  _e  ;  0_G_point  _c  ;  0_G_point  _g ;  0_G_point  _b  ]  ; 


%Number  of  points 
0_num=l 6; 
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%P re alio cat ion 


0_G_cent= 

=zeros (T, 6) ; 

0_C_cent= 

=  zeros  (T, 6) ; 

0_RotG2C= 

=zeros (3, 3, T) ; 

0_C_points=zeros  (0_num, 3, T) ; 
0_P_points=zeros  (0_num, 2, T) ; 
0_P_cent=zeros (T, 5) ; 

%Measurement  noise  covariances 


0_V_M  (1) 

=  obs  ( 1 )  ; 

0_V_M  (2) 

=  obs  (2 )  ; 

0_V_M  (3) 

=  obs  ( 3 ) ; 

%Match  inital  conditions 

0_G_cent  (1,  :  )  =  [T_G_cent  (1,  [1,2,3]),  T_G_cent  (1,  [1,2,3])]; 

%%  Particle  A  Parameters 


%Initial 

conditions 

xpl 

=  f ilterl ( 1 )  ; 

ypl 

=  f ilterl (2  )  ; 

zpl 

=  filterl  (3) ; 

Vpl 

=  filterl  ( 4 ) ; 

thetapl 

=  deg2rad (filterl  (5) ) ; 

phipl 

=  deg2rad ( filterl  (  6 )) ; 

dxpl 

filterl (7); 

dypl 

=  filterl  (  8 ) ; 

dzpl 

=  filterl  (  9) ; 

dVpl 

filterl  (10) ; 

dthetapl 

=  deg2rad ( filterl  ( 1 1 )) ; 

dphipl 

=  deg2rad (filterl (12) ) ; 
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xdotpl  =  Vpl*sin (thetapl) *cos (phipl) *dt; 

ydotpl  =  Vpl*cos (thetapl) *dt; 

zdotpl  =  Vpl*sin(thetapl)*sin(phipl)*dt; 

P_G_S (1, : ) = [xpl  ypl  zpl  Vpl  thetapl  phipl  dxpl  dypl  dzpl  dVpl  dthetapl . . . 
dphipl  xdotpl  ydotpl  zdotpl]; 

%Number  of  particles 
P.num  =  filterl(13); 


%Variances 


P_V_S  (1) 

= 

filterl  (14) ; 

%x  variance 

P_V_S  (2) 

= 

filterl  (15) ; 

%y  variance 

P_V_S (3) 

= 

filterl  (16) ; 

%z  variance 

P_V_S  (4) 

= 

filterl  (17) ; 

%V  variance 

P_V_S  (5) 

= 

filterl  (18) ; 

%theta  variance 

P_V_S  (6) 

= 

filterl  (19) ; 

%phi  varaince 

P_V_S (7) 

= 

filterl  (20) ; 

%dxp 

P_V_S  (8) 

= 

filterl  (21) ; 

%dyp 

P_V_S (9) 

= 

filterl  (22) ; 

%dzp 

P_V_S (10) 

= 

filterl  (23) ; 

%dVp 

P_V_S  (11) 

= 

filterl  (24) ; 

%dthetap 

P_V_S (12) 

= 

filterl  (25) ; 

%dphip 

P_V_M 

%Weighing 

filterl(26);  %Measureinent  noise  covar. 

matrix 

W_u  = 

filterl(27);  %u 

observation  weight 

W_v  = 

filterl (28) ;  %v 

observation  weight 

W_w  = 

filterl (29) ;  %w 

observation  weight 
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P_W  =  [W_u  0  0;  0  W_v  0;  0  0  W_w]; 

%Initial  particles  for  global  state 

P_G_S_part=zeros (P_num, 15, T) ;  %Preallocation  of  particle  matrix 
for  12=1 : P_num 

P_G_S_part  (12,  :,1)  =  [P_G_S  (1,1) tsqrt ( P _V_S  ( 1 )  ) *randn;  .  .  . 
P_G_S (1, 2) +sqrt (P_V_S (2) ) *randn; . . . 

P_G_S  (1, 3) tsqrt (P_V_S  (3) ) *randn;  .  .  . 

P_G_S  (1, 4) tsqrt (P_V_S  (4) ) *randn;  .  .  . 

P_G_S  (1, 5)  tsqrt  (P_V_S  (5)  )  *randn;  .  .  . 

P  _G_S  ( 1 , 6) tsqrt ( P  _V_S  ( 6 ) ) *randn;  .  .  . 

P_G_S (1, 7) tsqrt (P_V_S (7) ) *randn; . . . 

P_G_S  (1, 8) tsqrt (P_V_S  (8) ) *randn;  .  .  . 

P  _G_S  ( 1 , 9) tsqrt ( P  _V_S  ( 9 ) ) *randn;  .  .  . 

P_G_S (1, 10) tsqrt (P_V_S (10) ) *randn; . . . 

P_G_S  (1, 11) tsqrt (P_V_S  (11) ) *randn;  .  .  . 

P_G_S  (1,  12)  tsqrt (P_V_S  (12) ) *randn;  .  .  . 

P_G_S_part  (12, 10, 1) ;  .  .  . 

P_G_S_part  (12, 11, 1) ;  .  .  . 

P_G_S_part (12,12,1)]; 

end 

%P real location 

P_G_S_part_u=zeros (P_num, 15, T) ; 

P_RotG2C=zeros (3, 3, T) ; 

P_C_0_part=zeros (P_num, 3, T) ; 

P  _P_0_di f  f =zeros (P_num, 3, T) ; 

P_P_W=zeros (T, P_num) ; 

P_P_0_rawW=zeros  (T, P_num) ; 

P_P_get=zeros (T, P_num) ; 
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%%  Particle  B  conditions  (Up,Vp,Wp) 
%Initial  conditions 


xp2  = 

f ilter2 ( 1 ) ; 

Yp2 

filter2 (2) ; 

zp2  = 

f ilter2 (3) ; 

Vp2 

=  f ilter2 ( 4 ) ; 

thetap2 

=  deg2rad (f ilter2 (5) )  ; 

phip2 

=  deg2rad ( f ilter2 ( 6 ) )  ; 

dxp2  = 

filter2 (7) ; 

dyp2  = 

f ilter2 ( 8 ) ; 

dzp2  = 

f ilter2 ( 9) ; 

dVp2 

filter2(10); 

dthetap2 

=  deg2rad (filter2  (11) ) ; 

dphip2 

=  deg2rad (filter2  (12) ) ; 

xdotp2  = 

Vp2*sin (thetap2) *cos (phip2) *dt; 

ydotp2  = 

Vp2*cos (thetap2) *dt; 

zdotp2  = 

Vp2*sin (thetap2) *sin (phip2) *dt; 

P2_G_S  (1,  :  ) 

=[xp2  yp2  zp2  Vp2  thetap2  phip2  dxp2  dyp2  dzp2  dVp2  dthetap2 

dphip2  xdotp2  ydotp2  zdotp2]; 

%Number  of  particles 


P2_num  = 

filter2  (13) ; 

%Variances 


P2_V_S (1) 

=  filter2  (14) ;  %x  variance 

P2_V_S  (2) 

=  filter2(15);  %y  variance 

P2_V_S  (3) 

=  filter2(16);  %z  variance 

P2_V_S  (4) 

=  filter2(17);  %V  variance 

P2_V_S (5) 

=  filter2  (18) ;  %theta  variance 
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P2_V_S (6) 

filter2(19);  %phi  varaince 

P2_V_S  (7) 

= 

filter2 (20) ;  %dxp 

P2_V_S (8) 

= 

filter2  (21) ;  %dyp 

P2_V_S  (9) 

= 

filter2(22);  %dzp 

P2_V_S (10) 

= 

filter2  (23) ;  %dVp 

P2_V_S (11) 

= 

filter2(24);  %dthetap 

P2_V_S (12) 

= 

filter2(25);  %dphip 

P2_V_M  =  filter2(26);  %Measurement  noise  covariance 


%Weight  matrix 


W_u  = 

filter2 (27) ; 

%u 

observation 

weight 

W_v  = 

filter2 (28) ; 

%v 

observation 

weight 

W_s 

filter2 (29) ; 

%s 

observation 

weight 

W_udot  = 

filter2  (30) ; 

%wdot  observation  weight 

W_vdot  = 

filter2  (31) ; 

P2_W  =  [W_u  0000;  0  W_v  000;  00  W_s  00;  000  W_udot  0; . . . 
0000  W_vdot ] ; 

P2_W=[10  0  0  0  0;  .  .  . 

0  10  0  0  0;  .  .  . 

0  0  3  0  0 ;  .  .  . 

0  0  0  4  0 ;  .  .  . 


00004]; 

P2_W=diag ([10*ones{l,5)]); 


%Initial  particles  for  global  state 

P2_G_S_part=zeros {P2_num, 15, T) ;  %Preallocation  of  particle  matrix 
for  i2=l:P2_num 

P2_G_S_part  (i2,  : , 1)  =  [P2_G_S  (1, 1) +sqrt (P2_V_S  (1) ) *randn;  .  .  . 
P2_G_S  (1, 2) +sqrt (P2_V_S  (2) ) *randn;  .  .  . 
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P2_G_S  (1, 3) +sqrt (P2_V_S  (3) ) *randn;  .  . 
P  2  _G_S  (1,4) +sqrt ( P  2  _V_S  ( 4 ) ) *randn;  .  . 
P2_G_S (1, 5) +sqrt (P2_V_S (5) ) *randn; . . 
P  2  _G_S  ( 1 , 6) +sqrt ( P  2  _V_S  ( 6 ) ) *randn;  .  . 
P2_G_S  (1, 7) +sqrt (P2_V_S  (7) ) *randn;  .  . 
P2_G_S (1, 8) +sqrt (P2_V_S (8) ) *randn; . . 
P2_G_S (1, 9) +sqrt (P2_V_S (9) ) *randn; . . 
P2_G_S (1, 10) +sqrt (P2_V_S (10) ) *randn; 
P2_G_S (1,11) +sqrt (P2_V_S (11) ) *randn; 
P2_G_S (1,12) +sqrt (P2_V_S (12) ) *randn; 
P2_G_S_part  (12, 10, 1) ;  .  .  . 

P2_G_S_part  (12, 11, 1) ;  .  .  . 

P2_G_S_part  (12,12,1)]; 

end 

%P real location 

P2_G_S_part_u=zeros (P2_num, 15, T) ; 
P2_RotG2C=zeros (3, 3, T) ; 

P2_C_0_part=zeros  (P2_num,  6,  T)  ; 
P2_P_0_part=zeros  (P2_num,  5,  T)  ; 

P2_P_0_dif f=zeros (P2_num, 5, T) ; 

P2_P_W=zeros (T, P2_num) ; 


%%  SLMA 

Ll_G_cent=zeros (T, 9) ; 
Ll_G_cent (1, [1, 2, 3] ) 
Ll_G_cent (1,  [4, 5, 6] )  = 

Ll_G_cent (1, [7, 8, 9] )  = 

Ll_C_cent  (1,  [4, 5,  6]  )  = 

Ll_G_cent  (1,  : ) =T_G_cent  (1 


0_G_cent  (1,  [1,2,3]); 
T_G_cent  (1,  [13,14,15]); 
T_G_cent  (1,  [4, 5, 6] ) ; 
T_G_cent  (1,  [13,14,15]); 
[1,2,3,13,14,15,4,5,6]); 


%%  SLMB 

L2_G_cent=zeros (T, 9) ; 
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L2_G_cent (1,  [1,2,3]) 


0_G_cent  ( 1 ,  [1,2,3]); 


L2_G_cent (1,  [4,5, 6] ) 


T_G_cent  (1,  [13,14,15]); 


L2_G_cent (1,  [7, 8, 9] ) 


T_G_cent  (1,  [4,5,6]  )  ; 


L2_C_cent (1, [4,5, 6] ) 


T_G_cent  (1,  [13,14,15]); 


L2_G_cent (1,  :  )  =T_G_cent  (1,  [1,2,3,13,14,15,4,5,6]); 


L2_P_cent  =  zeros (T, 4); 

for  i=2:T  %i=l  is  initial  conditions 
%%  Truth  Centroid 

%Update  movement  of  centroid  in  global 
%Update  truth  centroid  position 

%Position  values  will  NOT  be  future  values:  the  x  used  here  is  based 
%off  of  the  previous  x  plus  the  previous  delta_x.  Thus,  delta  and  dot 
%values  pertain  to  the  x  in  the  same  time  step  NOT  a  future  x 
x=T_G_cent (i-1, 1) +T_G_cent (i-1, 7) ; 
y=T_G_cent (i-1, 2) +T_G_cent (i-1, 8) ; 
z=T_G_cent (i-1, 3) +T_G_cent (i-1, 9) ; 

V=T_G_cent (i-1, 4) +T_G_cent (i-1, 10) ; 
theta=T_G_cent (i-1, 5) +T_G_cent  (i-1,  11)  ; 
phi=T_G_cent (i-1, 6) +T_G_cent (i-1, 12) ; 

xdot=V*sin (theta) *cos (phi) *dt; 
ydot=V*cos (theta) *dt; 
zdot=V*sin (theta) *sin (phi) *dt; 

V_dot=T_G_cent (i-1, 10) /dt; 
theta_dot=T_G_cent (i-1, 11) /dt; 
phi_dot=T_G_cent (i-1, 12) /dt; 

dx=V*sin (theta) *cos (phi) *dtt . . . 

V_dot  *sin  (theta)  *cos(phi)*(dt''2/2)+.  .  . 
theta_dot*V*cos  (theta)  *cos  (phi)  *  (dt''2/2)  ; 
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dy=V*cos (theta) *dt+. . . 

V_dot *cos  (theta)  *  (dt''2/2)-.  .  . 
theta_dot*V*sin  (theta)  *  (dt''2/2)  ; 

dz=V*sin (theta) *sin (phi) *dt+  .  .  . 

V_dot  *sin  (theta)  *sin(phi)*(dt''2/2)+.  .  . 
phi_dot*V*sin  (theta)  *cos  (phi)  *  (dt''2/2) 

dV=V_dot  *dt ; 
dtheta=theta_dot  *dt ; 
dphi=phi_dot  *dt ; 

%Add  Process  Noise 
V=V+sqrt (TS_V (1) ) *randn; 
theta=theta+sqrt (TS_V (2) ) *randn; 
phi=phi+sqrt (TS_V (3) ) *randn; 

%New  centroid  positions 
T_G_cent  (i, 1) =x; 

T_G_cent  (i, 2) =y; 

T_G_cent  (i,3)=z; 

T_G_cent (i, 4) =V; 

T_G_cent  (i, 5) =theta; 

T_G_cent  (i, 6) =phi; 

T_G_cent  (i, 7) =dx; 

T_G_cent  (i, 8) =dy; 

T_G_cent  (i, 9) =dz; 

T_G_cent  (i, 10) =dV; 

T_G_cent  (i, 11) =dtheta; 

T_G_cent  (i, 12) =dphi; 

T_G_cent  (i, 13) =xdot; 
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T_G_cent  (i, 14) =ydot; 

T_G_cent  (i, 15) =zdot; 

%Rotate  to  camera  orientation 

%States  in  the  camera  orientation  are  u,v,w  and  dots 
%Generate  angles  and  rotation  DCM 

%Camera  angles  assume  the  camera  tracks  the  truth  centroid  perfectly 
T_G_Rotdata  (i,  1)  =sqrt  (T_G_cent  (i,l)  ''2  +  T_G_cent  (1,2)  ''2  +  T_G_cent  (i,3)  "2)  ; 
%Pan  (phi) 

T_G_Rotdata  (i, 2) =atan2  (T_G_cent  (i, 3) ,  T_G_cent  (i, 1) ) ; 

%Tilt,  note:  tilts  from  y  axis  (positive  down),  so  0  is  on  y— axis 
% (theta) 

T_G_Rotdata  (i,3)=acos  (T_G_cent  (i,2)  /T_G_Rotdata  (i,  1)  )  ; 

%Calculate  rate  of  rotation  (phi  dot) ,  x 

T_G_Rotdata  (i,  4)  =  (T_G_Rotdata  (i,  2)  -T_G_Rotdata  (i-1, 2)  )  /dt; 

%Calculate  rate  of  tilt  (theta  dot) ,  y 

T_G_Rotdata  (i,  5)  =  (T_G_Rotdata  (i,  3)  -T_G_Rotdata  (i-1, 3)  )  /dt; 

%Create  DCM  based  on  current  rotation  angles 

T_DCM_G2C=@ (angz)  Rot_y (angz (1) —pi/ 2) *Rot_x (angz (2) —pi/2) *Rot_z (pi/ 2) ; 
T_RotG2C  (  :  ,  : , i ) =T_DCM_G2 C (T_G_Rotdata (i,  [2, 3] ) )  .  ' ; 

%Rotate  centroid  to  camera  frame 

T_C_cent(i, [1,2,3]) =T_RotG2C (:,:,i)*T_G_cent(i, [1,2,3]) %u,v,w 
T_C_cent(i,  [4,5,6]) =T_RotG2C (:,  :,i)*T_G_cent  (i,  [13,14,15])  %dot  u  v  w 

%Convert  to  pixel  values 

T_P_cent  (i,  1)  =  (  (T_C_cent  (i, 1) /T_C_cent  (i, 3) )  ) *f;  %u_p 
T_P_cent  (i,2)  =  (  (T_C_cent  (i,2)/T_C_cent  (i,3)  )  )*f;  %v_p 
T_P_cent  (i,  3)  =  (T_C_cent  (i,  6)  /  T_C_cent  (i,  3)  )  *f;  %w 
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T_P_cent  (i,4)  =  {T_C_cent  (1,4)  /  T_C_cent  (i,3)  )*f;  %u_pdot 
T_P_cent  (i,5)  =  {T_C_cent  (1,5)  /  T_C_cent  (i,3)  )  *f;  %v_pdot 


%%  Observation  Centroid  and  Points 
%Update  global  point  position  (CUBE) 
for  12=1 :  0_nuin 

0_G_points  (12, 1, i) =0_G_points  (12, 1, i-1) +T_G_cent  (i-1, 7) ; 
0_G_points  (i2,2,i)=0_G_points  (i2,2,i-l)+T_G_cent  (i-1, 8) ; 
0_G_points  (i2, 3, i) =0_G_points  (12, 3, i-1) +T_G_cent  (i-1, 9) ; 

end 

%Calculate  observed  centroid  based  on  mean  of  points 
0_G_cent  (i, 1) =mean (0_G_points ( : , 1, i) ) ; 

0_G_cent (i, 2) =mean (0_G_points ( : , 2, i) ) ; 

0_G_cent  (i, 3) =mean (0_G_points ( : , 3, i) ) ; 

%Rotate  to  camera  orientation  (use  same  cam  rot  angles  as  in  truth) 
%Rotate  points  to  camera  orientation 
0_RotG2C  (  :  ,  : , i) =T_RotG2C ( : ,  : , i) ; 

O_noise= [sqrt (0_V_M (1)  ) *randn;  sqrt (0_V_M (2)  ) *randn;  .  .  . 

sqrt (0_V_M  (3) ) *randn] ; 
for  12=1 : 0_num 

0_C  .points  (i2,  :,i)  =0_RotG2C  (  :  ,  :  ,  i)  *0_G_points  (i2,  :,i)  'fO.noise; 

end 

%Calculate  observed  centroid  (in  camera  frame) 

0_C_cent  (i, 1) =mean (0_C_points ( : , 1, i) ) ; 

0_C_cent (i, 2) =mean (0_C_points ( : , 2, i) ) ; 

O.C.cent  (i, 3) =mean (O.C.points ( : , 3, i) ) ; 

%Calculate  u,v,w  velocities  (difference  between  the  two  positions) 


248 


%Assume  all  points  move  with  the  same  velocites,  so  we  use  the  truth 
%velocites,  u  v  w  dot 

0_C_cent  (1,  [4,5, 6] )=  T_C_cent  (1,  [4,5, 6] ) ; 

T_RotC2G  (  : ,  : , 1) =transpose {T_RotG2C { : ,  :,!)); 

0_G_cent(i, [4,5,6]) =T_RotC2G (:,:,i)*0_C_cent(i, [1,2,3]) 


Calculate  and  convert  to  pixel  values 

%Calculate  u  and  v  pixel  values  for  each  point 
for  12=1 : 0_num 

0_P -points  (12, l,i)  =  {0_C -points  (12, l,i)/0-C -points  (12,3,1)  )*f;  %U-p 
0-P -points  {i2,2,i)  =  {0-C-points  {i2,2,i)/0-C-points  (12,3,1)  )*f;  %V-p 

end 

%Calculate  s,  U-dot,  v-dot  using  least  mean  squares  method 
St  at  e=[0_P -points  (:, 1,1-1)  '  0_P -points  (:,2,i-l)  .  .  . 

zeros (1, 0-num)  dt*ones (1, 0-num) ; . . . 
dt*ones  (1, 0-num)  zeros (1, 0-num)  ] ; 

b=  [0-P -points  (:,l,i)  '  0-P -points  (:,2,i)  ']  ; 

A=b*state ' *pinv (state*state  '  )  ; 

A=b*  (state'*state)''-l*state'*state; 

%Create  pixel  'centroid' 


O-P-Cent  (1,1) 

=mean  (0-P -points  (  :  ,  1,  i)  )  ; 

%U-P 

0-P-Cent (1,2) 

=mean  (0-P -points  (  :  ,  2, 1)  )  ; 

%V-P 

0-P-cent  (1,3) 

=A ( 1 , 1 )  ; 

%s 

0-P-cent (1,4) 

=A ( 1 , 2 ) ; 

%U-P-dot 

0-P-Cent (1,5) 

=A(1,3) ; 

%v-p-dot 

%  Particle  Centroid  and  Points  (U,V,W) 
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Update  particles  (state  and  observations) 

As  with  the  Truth  cent,  take  in  the  previous  time  step  values 
for  12=1 : P_num 

%Update  state 

x=P_G_S_part (i2,l,i-l)+P_G_S_part(i2,7,i-l); 
y=P_G_S_part (i2, 2, i-1) +P_G_S_part (i2,8,i-l); 
z=P_G_S_part  (i2, 3, i-1) +P_G_S_part  (i2,  9, i-1) ; 

V=P_G_S_part  (i2,4,i-l)+P_G_S_part  (12,10, i-1) fsqrt ( P  _V_S  ( 4 ) ) *randn; 
theta=P _G_S_part  (i2,5,i-l)+P_G_S_part  (12, 11, i-1) +.  .  . 
sqrt (P_V_S (5) ) *randn; 

phi=P _G_S_part  (i2,6,i— l)+P_G_S_part  (i2,12,i— 1)+.  .  . 
sqrt (P_V_S (6) ) *randn; 

%Angle  adjustment:  ensure  V  is  not  negative,  angles  in  proper  range 
if  (V<0) 

V=abs (V) ; 

end 

%Fix  and  reduce  angles 
theta=wrapTo2Pi (theta)  ; 
if (theta>pi ( ) ) 

theta_a=theta-pi ()  ; 
theta=pi ( ) -theta_a  ; 

end 

xdot=V*sin (theta) *cos (phi) *dt; 
ydot=V*cos (theta) *dt; 
zdot=V*sin (theta) *sin (phi) *dt; 

V_dot=P_G_S_part (i2, 10, i) /dt; 
theta_dot=P_G_S_part (12, 11, i) /dt; 
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phi_dot=P_G_S_part (i2, 12, i) /dt; 


dx=V*sin (theta) *cos (phi) *dtt . . . 

V_dot*sin  (theta)  *cos  (phi)  *(dt''2/2)  +  .  .  . 
theta_dot *V*cos  (theta)  *cos  (phi)  *  (dt''2/2)  + 
-phi_dot  *V*sin  (theta)  *sin  (phi)  *  (dt''2/2)  ; 

dy=V*cos (theta) *dtt . . . 

V_dot *cos  (theta)  *  (dt''2/2)-.  .  . 
theta_dot*V*sin  (theta)  *  (dt''2/2)  ; 

dz=V*sin (theta) *sin (phi) *dtt . . . 

V_dot*sin  (theta)  *sin  (phi)  *(dt''2/2)  +  .  .  . 
theta_dot *V*cos  (theta)  *sin  (phi)  *  (dt''2/2)  + 
phi_dot*V*sin  (theta)  *cos  (phi)  *  (dt''2/2)  ; 

dV=V_dot  *dt  ; 
dtheta=theta_dot  *dt  ; 
dphi=phi_dot  *dt ; 

%New  centroid  positions 
P_G_S_part_u  (i2,l,i)=x; 

P_G_S_part_u  (i2,2,i)=y; 

P_G_S_part_u  (i2,3,i)=z; 

P_G_S_part_u (i2, 4, i) =V; 

P_G_S_part_u (i2, 5, i) =theta; 

P_G_S_part_u (i2, 6, i) =phi; 

P_G_S_part_u (i2, 7, i) =dx; 

P_G_S_part_u (i2, 8, i) =dy; 

P_G_S_part_u  (i2,9,i)=dz; 

P_G_S_part_u (i2, 10, i) =dV; 

P_G_S_part_u (i2, 11,  i) =dtheta; 
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P_G_S_part_u (i2, 12, i) =dphi ; 

P_G_S_part_u (12, 13, i) =xdot; 

P_G_S_part_u (12, 14, i) =ydot; 

P_G_S_part_u (12, 15, i) =zdot ; 

%Observation  Update  (ie.  what  we  think  the  camera  will  see  based  on 
%the  states) 

%Rotate  to  camera  frame  (use  same  angles  as  in  Truth) 

P_RotG2C  (  :  ,  : , i) =T_RotG2C ( : ,  : , i) ; 

P_C_0_part  (i2,  [1,2,3] , i) =P_RotG2C ( : ,  : , i) *  .  .  . 

P_G_S_part_u (i2,  [1, 2, 3] , i)  '  ; 

%Calculate  difference  between  observation  (measurement)  and  filter 
%predicition 

P_P_0_diff  (i2,  :,i)=0_C_cent  (i,  [1,2,3] )-P_C_0_part  (i2,  :,i) ; 

%Weights  to  be  used 

P_P_0_rawW  (i,i2)=P_P_0_diff  (i2,  :,i)*P_W*P_P_0_diff  (12,  :,i) 

%Weight  particles 

P_P_W  (i,i2)  =  (l/ sqrt (2*pi*P_V_M) ) *exp (— (P_P_0_diff (12,  :,i)*P_W*.  .  . 
P_P_0_dif f  (i2,  : , i)  ' ) /  (2*P_V_M) ) ; 

end 

%Normalize  to  form  a  probability  distribution  (ie.  sums  to  1) 

P_P_W  (i,  : ) =P_P_W (i,  : )  . /sum(P_P_W (i,  : ) )  ; 

%Resampling:  from  this  new  distribution,  we  randomly  resample  from  it 
%to  generate  new  estimate  particles 
for  12=1 : P_num 

P  _P_get  ( i , 12 ) =f ind ( rand<=cumsum ( P  _P  _W (i,  : ) ) , 1 ) ; 

P_G_S_part  (i2,  :,i)=P_G_S_part_u  (P_P_get  (i,i2)  ,  :,i)  ; 
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end 


%The  final  estimate,  state,  is  a  metric  of  the  final  resampling 
P_G_S (i, : ) =mean (P_G_S_part ( : , : , i) ) ; 

%%  Particle  Centroid  and  Points  (Up,Vp,Wp) 

%Select  model  variation/parameter 
%Noise  on  vector:  1 

%Jacobain:  2 

%No  correction:  0 

%Update  particles  (state  and  observations) 

%As  with  the  Truth  cent,  take  in  the  previous  time  step  values 
switch  correct 

case  1  %Noise  on  vector 

temp_r_vec=zeros (1,3) ; 

[temp_r_vec  (1)  ,  temp_r_vec  (2)  ,  temp_r_vec  (3)  ]  =  sph2cart  (  .  .  . 

T_G_Rotdata  (i,  2)  ,  T_G_Rotdata  (i,  3)  ,  T_G_Rotdata  (i,  1)  )  ; 
temp_r_vec=temp_r_vec/norm  (temp_r_vec)  *7*randn; 

%  temp_r_vec  =  T_RotG2C (end, : , i) *7*randn; 

temp_r_vec  =  T_RotG2C(end,:,i); 

temp_randVtp= [ . l*sqrt (P2_V_S (4) )  0  0;  0  . l*sqrt (P2_V_S (5) )  0;... 

0  0  . l*sqrt (P2_V_S  (6) ) ] ; 


case  2 

temp_r_vec=zeros (1, 3) ;  %  Prevents  any  noise  being  introduced 
if  i>3 

%Retrieve  previous  estimated  global  state  values,  reassign  to  variables 
%for  ease  of  calculation  and  jacobian  generation 

V  =  P2_G_S (i-1, 4) ; 

theta  =  P2_G_S (i-1, 5) ; 

phi  =  P2_G_S (i-1, 6) ; 
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%First  Jacobian,  uses  V,  theta,  phi,  and  returns  dx,  dy,  dz 
V_Jacobl  (:,  :,i)  =  [cos (theta) *sin (phi) *dt,  -V*sin (theta) * .  .  . 
sin (phi) *dt,  V*cos (theta) *cos (phi) *dt;  .  .  . 
cos(phi)*dt,  0,  -V*sin (phi) *dt; . . . 

sin (theta) *sin (phi) *dt,  V*cos (theta) *sin (phi) *dt, . . . 

V*cos  (phi) *sin (theta) *dt]  ; 

%Second  Jacobian 
w  =  P2_C_S (i-1, 3) ; 

V_Jacob2  (  : ,  : , i )  =  [ 0 ,  0,  1/w;  1/w,  0,  0;  0,  1/w,  0 ]  ; 

%Generate  full  matrix 

V.JacobAll  (  :  ,  :  , i)  =  V_Jacob2  (  :  ,  :  , i) *P2_RotG2C  (  :  ,  : , i-1) * .  .  . 
V_Jacobl  (  :  ,  :  , i ) ; 

%Generate  variance  values,  based  on  eigen  values  that 
%define  the  amount  of  correlation  between  state  and 
%variable  and  the  vectors  that  indicate  if  this 
%correlation  is  increasing 

[temp_Evec, temp_Eval] =eig  (V_JacobAll  (:,:, i ).'*..  . 

V_JacobAll  (  :  ,  :  ,  i)  )  ; 

temp_randVtp=10*temp_Evec*diag ( 4e— 5* (diag ( temp_Eval ) t . . . 

4e-6)  .  ''-!)  ; 

else 

temp_Evec ( : , 1 )  =  [  1 ; 0 ; 0 ] ; 
temp_Eval ( 1 , 1 ) =le—4; 
temp_Evec ( :  ,  2 )  =  [  0 ; 1 ; 0 ] ; 
temp_Eval (2,2) =le—2; 
temp_Evec ( : , 3 )  =  [0;0;1]; 
temp_Eval (3,3) =le-2; 

temp_randVtp=3*temp_Evec*diag (4e— 5* (diag (temp_Eval) t4e— 6)  . “-I)  ; 

end 

case  0 

temp_r_vec=zeros (1,3) ; 
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temp_randVtp= [ . l*sqrt {P2_V_S (4) )  0  0;  0  . l*sqrt (P2_V_S (5) )  0;... 


0  0  . l*sqrt (P2_V_S  (6) ) ] ; 


end 

for  i2=l:P2_num 

%Generate  variances 

temp_r_vec=temp_r_vec . * [sqrt (P2_V_S (1) ) *randn, . . . 

sqrt (P2_V_S (2) ) *randn,  sqrt {P2_V_S (3) ) *randn] ; 
temp_randVtp2=temp_randVtp*randn (3,1) ; 

%Update  state 

x=  P2_G_S_part  (i2,  1,  i— 1) +P2_G_S_part  (i2,  7  ,  i— 1 ) +temp_r_vec  ( 1 ) ; 

y=  P2_G_S_part  (12, 2,  i-1) +P2_G_S_part  (12,  8, 1-1) +temp_r_vec  (2)  ; 

z=  P2_G_S_part  (12, 3, 1-1)  tP2_G_S_part  (12,  9, 1-1) +temp_r_vec  (3)  ; 

V=  P2_G_S_part (12,4,1-1) +P2_G_S_part (12,10, 1-1) +. . . 
temp_randVtp2  (1); 

theta=P  2_G_S_part  (12,5,1— l)tP2_G_S  _p  art  (12,11,1-1)+.  .  . 
teinp_randVtp2  (2)  ; 

phi=  P2_G_S_part  (12,6,1— 1)+P2_G_S _p art  (12,12,1—1)+.  .  . 
teinp_randVtp2  (3)  ; 

%Angle  adjustment:  ensure  V  is  not  negative,  angles  in  proper  range 
if  (V<0) 

V=abs (V) ; 

end 

xdot=V*sin (theta) *cos (phi) *dt; 
ydot=V*cos (phi) *dt; 
zdot=V*sin (theta) *sin (phi) *dt; 
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V_dot=P2_G_S_part (12, 10, i-1) /dt; 
theta_dot=P2_G_S_part (12, 11, i-1) /dt; 
phi_dot=P2_G_S_part (12, 12, i-1) /dt; 

dx=V*sin (theta) *cos (phi) *dt+ . . . 

V_dot*sin  (theta)  *cos  (phi)  *(dt''2/2)  +  .  .  . 
theta_dot *V*cos  (theta)  *cos  (phi)  *  (dt''2/2)  + 
— phi_dot  *V*sin  (theta)  *sin  (phi)  *  (dt''2/2)  ; 

dy=V*cos (theta) *dt+ . . . 

V_dot *cos  (theta)  *(dt''2/2)-.  .  . 
theta_dot*V*sin  (theta)  *  (dt''2/2)  ; 

dz=V*sin (theta) *sin (phi) *dt+ . . . 

V_dot*sin  (theta)  *sin  (phi)  *(dt''2/2)  +  .  .  . 
theta_dot *V*cos  (theta)  *sin  (phi)  *  (dt''2/2)  + 
phi_dot*V*sin  (theta)  *cos  (phi)  *  (dt''2/2)  ; 

dV=V_dot  *dt  ; 
dtheta=theta_dot  *dt  ; 
dphi=phi_dot  *dt ; 

%New  centroid  positions 
P2_G_S_part_u  (i2,l,i)=x; 

P2_G_S_part_u  (i2,2,i)=y; 

P2_G_S_part_u  (i2,3,i)=z; 

P2_G_S_part_u (i2, 4, i) =V; 

P2_G_S_part_u (i2,5,i) =theta; 

P2_G_S_part_u (i2, 6, i) =phi; 

P2_G_S_part_u (i2, 7, i) =dx; 

P2_G_S_part_u (i2, 8, i) =dy; 

P2_G_S_part_u (i2, 9, i) =dz; 
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P2_G_S_part_u (i2, 10, i) =dV; 

P2_G_S_part_u (12, 11, i) =dtheta; 

P2_G_S_part_u (12, 12, 1) =dphl; 

P2_G_S_part_u (12, 13, 1) =xdot; 

P2_G_S_part_u (12, 14, 1) =ydot; 

P2_G_S_part_u (12, 15, 1) =zdot; 

%Observatlon  Update  (le.  what  we  think  the  camera  will  see  based  on 
%the  states) 

%Rotate  to  camera  frame  (use  same  angles  as  In  Truth) 

P2_RotG2C ( : , : , 1) =T_RotG2C ( : , :,1); 

P2_C_0_part  (12,  [1,2,3]  ,  1)  =P2_RotG2C  (:,:,!)*... 

P2_G_S_part_u(12,  [1,2,3],!) 

P2_C_0_part  (12,  [4,5,6]  ,  1 )  =P2_RotG2C  (:,:,!)*... 

P2_G_S_part_u(12,  [13,14,15],!) 

%Obtaln  pixel  values,  u, v, s, udot, vdot,  based  on  camera  states 
P2_P_0_part  (12,l,l)=P2_C_0_part  (12,l,l)/P2_C_0_part  (12,3,l)*f;  %u_p 
P2_P_0_part  (12,2,l)=P2_C_0_part  (12,2,l)/P2_C_0_part  (12,3,l)*f;  %v_p 
P2_P_0_part  (12,3,l)=P2_C_0_part  (12,6,l)/P2_C_0_part  (12,3,l)*f;  %new  s 
P2_P_0_part  (12, 3, !)  =  (-(  (P2_C_0_part  (12,  1,1)  *.  .  . 

P2_C_0_part  (12, 6,1)  )  /  P2_C_0_part  (12,3,1)  "'2)  +  .  .  . 

-(  (P2_C_0_part  (12,2,1)  *P2_C_0_part  (12, 6,1)  )  /  .  .  . 
P2_C_0_part(12,3,l)''2))/2;  %s_p 

P2_P_0_part  (12,4,l)  =  (P2_C_0_part  (12, 4,1)  /P2_C_0_part  (12, 3,1)  )  *f; 
P2_P_0_part  (12,5,l)  =  (P2_C_0_part  (12, 5,1)  /P2_C_0_part  (12, 3,1)  )  *f; 

%Calculate  difference  between  observed  centroid  and  particle  cent 
P2_P_0_dlff  (12,  :,l)=0_P_cent  (1,  :)-P2_P_0_part  (12,  :,1)  ; 

P2_P_0_dlff  (12,  :,l)=T_P_cent  (1,  :)-P2_P_0_part  (12,  :,1)  ; 

%Welghts  to  be  used 
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P2_P_0_rawW  (i,i2)=P2_P_0_diff  (i2,  :,i)*P2_W*P2_P_0_diff  (i2,  :,i 


%Weight  particles 

P2_P_W (i,i2)  =  (l/ sqrt {2*pi*P2_V_M) ) *exp {- (P2_P_0_diff {i2,  :  ,  i) * 
P2_W*P2_P_0_dif f (12, : , i) ' ) / {2*P2_V_M) ) ; 

end 

stop=mean ( sum { P 2 _P _W  (i ,  :  )  )  )  ; 
if  stop  <  le-175 
stop; 
f lag=l ; 
break 

end 

P2_P_0_dif f  (i2,  : , i) 

Plot  distribution  of  weighted  particles  in  real  time 
for  diagnostic  purposes 
figure (72) ; clf; 

set (gcf,  ' position ',  [  680  72  1152  906]); 
a=subplot (3,2,1); 

stem3  (P2_P_0_part  (  :  ,  1,  i)  ,  P2_P_0_part  (  :  ,  2,  i)  ,  P2_P_W  (i,  :  )  ,  '  ro  '  )  ; 
hold  on ;  stem3 (T_P_cent (i, 1) , T_P_cent (1,2) , max ( P 2 _P _W ( i,  : ) ) ,  ' k* ' ) 
title (a, 'U  vs  V  Position  Particle  Weight'); 

daspect ( [lei, lei, le-2] ) ; drawnow; 
a=subplot (3,2,3) ; 

stem  (P2_P_0_part  (  : ,  3,  i)  ,  P2_P_W  (i,  :  )  ,  '  ro  '  )  ; 

hold  on;  stem  (  T_P_cent  ( i , 3 ) , max ( P 2 _P_W ( i,  : ) )  ,  ' k* ' ) ; 

title  (a,  'Z  Position  Particle  Weight'); 

daspect ( [ le-4 , le-2 , 1 ] ) ; drawnow; 
a=subplot (3,2,5) ; 

stem3  (P2_P_0_part  (  :  ,  4,  i)  ,  P2_P_0_part  (  :  ,  5,  i)  ,  P2_P_W  (i,  :  )  ,  '  ro  '  )  ; 
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hold  on;  stem3 {T_P_cent (i, 4) , T_P_cent (i, 5) , max { P 2 _P _W ( i,  : ) ) ,  ' k* ' ) ; 
title (a,  'U  vs  V  Velocity  Particle  Weight'); 

%  daspect ( [le2, le2, le-2] ) ; drawnow; 

a=subplot (3,2,2)  ; 

stem3  (P2_G_S_part_u(:,l,i),P2_G_S_part_u(:,2,i),P2_P_W(i,  :),  'ro'); 
hold  on;  stem3 {T_G_cent (1, 1) , T_G_cent (1,2) , max ( P 2 _P _W ( 1 ,  : ) ) ,  ' k* ' ) ; 
title (a,  'X  vs  Y  Position  Particle  Weight'); 

%  daspect ( [lei, lei, le-2] ) ;drawnow; 

a=subplot (3,2,4)  ; 

stem(P2_G_S_part_u(:,3,i),P2_P_W(i,  :),  'ro'); 

hold  on;  stem  (  T_G_cent  ( i , 3 ) , max ( P 2 _P _W ( i,  : ) )  ,  ' k* ' ) ; 

title  (a,  'Z  Position  Particle  Weight'); 

%  daspect ( [lei, lei, le-2] ) ;drawnow; 

a=subplot (3,2,6); 

stem3  (ang_red  (P2_G_S_part_u  (:,5,i)  )  ,  ang_red  (P2_G_S_part_u  (:,  6,i)  )  ,  .  .  . 
P2_P_W  (i,  :  )  ,  '  ro  '  )  ; 

hold  on;  stem3 (ang_red (T_G_cent (i, 3) ) , ang_red (T_G_cent (i, 4) ) , . . . 

max ( P  2  _P_W ( i ,  : ) ) ,  ' k* ' ) ; 
title (a,  'X  vs  Y  Velocity  Particle  Weight'); 

%  daspect ( [lei, lei, le-2] ) ; drawnow; 

pause ( )  ; 

9-  9- 
o  o 

if  (flag==l) 
break 

end 

%  P2_P_0_dif  fmean  (i,  :  )  =mean  (P2_P_0_dif  f  (  :  ,  :  ,  i)  )  ; 

%Normalize  to  form  a  probability  distribution  (ie.  sums  to  1) 

P2_P_W  (i,  :  )  =P2_P_W  (i,  :  )  .  /sum(P2_P_W  (i,  :  )  )  ; 

%Resampling:  from  this  new  distribution,  we  randomly  resample  from  it 
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%to  generate  new  estimate  particles 


for  i2=l:P2_num 

P2_P_get  (i,i2) =f ind { rand<=cumsum { P 2 _P _W (i,  :  )  )  ,  1 )  ; 
P2_G_S_part  (i2,  :,i)=P2_G_S_part_u  (P2_P_get  {i,i2)  ,  :,i)  ; 

end 

%The  final  estimate,  state,  is  a  metric  of  the  final  resampling 
P2_G_S (i, : ) =mean (P2_G_S_part ( : , : , i) ) ; 

%Provide  P2_C_S  for  variance  calculations 

P2_C_S  (i,  [1,2,3]) =P2_RotG2C  (  : ,  : , i) *P2_G_S  (i,  [4,5,6]) 

P2_C_S  (i,  [4,5,6]) =P2_RotG2C ( : ,  :  ,  i) *P2_G_S  (i,  [10,11,12]) 


SLMA 

Ll_C_cent  (i,  1) 
Ll_C_cent  (i,  2) 
Ll_C_cent  (i,  3) 


0_C_cent  (i-1, 1) +Ll_C_cent (i-l, 4) *dt; 
0_C_cent  (i-l,2)+Ll_C_cent (i-l, 5) *dt; 
0_C_cent  (i-l,3)+Ll_C_cent (i-l, 6) *dt; 


%Velocities 
Ll_C_cent  (i, 4) 
Ll_C_cent  (i,  5) 
Ll_C_cent  (i,  6) 


(0_C_cent  (i, l)-0_C_cent  (i-l, 1) ) /dt; 
(0_C_cent  (i,2)-0_C_cent  (i-l, 2) ) /dt; 
(0_C_cent  (i, 3) -0_C_cent  (i-l, 3) ) /dt; 


%Rotate  to  global  from  camera  (inverse  of  DCM) 

Ll_G_cent  (i,  [1,2,3]) =T_RotC2G ( : ,  : , i) *Ll_C_cent  (i,  [1,2,3])  '  ; 

Ll_G_cent  (i, 4)  =  (Ll_G_cent (i, l)-Ll_G_cent (i-l, 1) ) /dt; 
Ll_G_cent  (i,5)  =  (Ll_G_cent  (i,2)-Ll_G_cent (i-l, 2) )/dt; 
Ll_G_cent  (i,  6)  =  (Ll_G_cent  (i,3)-Ll_G_cent  (i-l,  3)  )  /dt; 
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V  =  sqrt  (Ll_G_cent  (i,  4)  ■'2  +  Ll_G_cent  (i,  5)  ''2  +  Ll_G_cent  (i,  6)  "2)  ; 

theta  =  acos (Ll_G_cent (i, 5) /V) ; 

phi  =  atan2 (Ll_G_cent (i, 6) ,  Ll_G_cent  (i, 4) ) ; 

Ll_G_cent  (i,  7)  =  V; 

Ll_G_cent  (i, 8)  =  theta; 

Ll_G_cent  (i,  9)  =  phi; 

%  SLMB 

Update  up,  vp,  s  (w) 

%Note:  normally  w=s*scale  factor  of  target,  but  since  the  obs  points 
%are  from  the  edge  of  the  target,  this  scale  factor  =1,  so  it  is  not 
%explicitly  stated 

%Find  u,  w,  v 

L2_C_cent  (i, 1)  =  (0_P_cent  (i, 3) *0_P_cent  (i, 1) ) /f ; 

L2_C_cent  (i, 2)  =  (0_P_cent  (i, 3) *0_P_cent  (i,  2) ) /f ; 

L2_C_cent  (i,  3)  =  sqrt  (  (  {-L2_C_cent  (i,  1) -L2_C_cent  (i,  2)  )  *  .  .  . 

L2_C_cent (i-l, 6) ) /  (2*0_P_cent  (i, 3) ) ) ; 

L2_C_cent  (i, 4)  =  (0_P_cent  (i, 3) *0_P_cent  (i, 4) ) /f ; 

L2_C_cent  (i, 5)  =  (0_P_cent  (i, 3) *0_P_cent  (i, 5) ) /f ; 

L2_C_cent  (i, 6)  =  (0_P_cent  (i, 3) -0_P_cent  (i-l, 3) ) /dt; 

L2_C_cent  (i,  :  )  ; 

%Rotate  to  global  from  camera  (inverse  of  DCM) 

L2_G_cent  (i,  [1,2,3]) =T_RotC2G ( : ,  : , i) *L2_C_cent  (i,  [1,2,3])  '  ; 

L2_G_cent (i, 4)=(L2_G_cent (i, l)-L2_G_cent (i-l, 1) ) /dt; 

L2_G_cent (i,5)=(L2_G_cent (i,2)-L2_G_cent (1-1,2) )/dt; 

L2_G_cent  (i, 6)  =  (L2_G_cent (i,3)-L2_G_cent (i-l, 3) ) /dt; 
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V 


sqrt  (L2_G_cent  (i,4)  ■'2  +  L2_G_cent  (i,5)  ''2  +  L2_G_cent  (i,  6)  "2)  ; 


theta 


acos (L2_G_cent  (i, 5) /V)  ; 


phi 


atan2 (L2_G_cent (i, 6) ,  L2_G_cent  (i, 4) ) ; 


L2_G_cent  (i, 7) 


V; 


L2_G_cent  (i, 8) 


theta; 


L2_G_cent  (i, 9) 


phi; 


end 


%Angle  adjustment:  ensure  V  is  not  negative,  angles  in  proper  range 
for  i=2  :  T 

%Terminate  if  EPF— B  collapses 
if { f lag==l ) 
break 

end 

%Target 

T_G_cent  (i, 5) =wrapTo2Pi {T_G_cent  (i, 5) ) ; 

T_G_cent  (i, 6) =wrapTo2Pi {T_G_cent  (i, 6) ) ; 

if{T_G_cent  (i,4)<0) 

T_G_cent (i, 4) =abs (T_G_cent (i, 4) ) ; 

T_G_cent { i , 6 ) =wrapTo2Pi ( T_G_cent { i , 6 ) +pi ( ) ) ; 

T_G_cent  {i,5)=pi{)-T_G_cent  (i,5)  ; 


end 


%Fix  and  reduce  angles 
if{T_G_cent{i,5)  >pi  ( )  ) 

theta _a=T_G -Cent  (i,5)  -pi  ( )  ; 

T-G_cent { i , 5 ) =pi ( ) -theta_a; 

T_G_cent  { i , 6 ) =wrapTo2Pi ( T_G_cent ( i, 6 ) +pi { ) ) ; 


end 
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%EPF-A 

%Only  need  to  adjust  phi,  v  is  already  only  absolute  and  theta  has 

%already  been  constrained 

%D0  NOT  USE  WRAPT02PI! ! ! 

phi=P_G_S  (i, 6) ; 

if  (phi  >  2*pi { ) ) 

mult=f loor (phi/ (2*pi { ) ) ) ; 
phi=phi— mult*2*pi ()  ; 

end 

if  (phi  <  -2*pi  ( ) ) 

mult=floor{phi/ {-2*pi  {) ) ) ; 
phi=phi+mult*2*pi  () ; 

end 

if  (phi  <  0) 

phi=2*pi 0  +phi; 

end 

P_G_S  (i, 6) =abs (phi) ; 


%EPF-B 

%Only  need  to  adjust  phi,  v  is  already  only  absolute  and  theta  has 

%already  been  constrained 

%DO  NOT  USE  WRAPT02PI! ! ! 

phi=P2_G_S  (i, 6) ; 

if  (phi  >  2*pi { ) ) 

mult=f loor (phi/ (2*pi { ) ) ) ; 
phi=phi— mult*2*pi ()  ; 

end 
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if  (phi  <  -2*pi ( ) ) 

mult=floor{phi/ {-2*pi  () ) ) ; 
phi=phi+mult*2*pi  () ; 

end 

if  (phi  <  0) 

phi=2*pi 0  +phi; 

end 

P2_G_S (i, 6) =abs (phi) ; 


%SLMA 

%Angle  adjustment:  ensure  V  is  not  negative,  angles  in  proper  range 

phi=Ll_G_cent  (i, 9) ; 

if  (phi  >  2*pi  { ) ) 

mult=f loor (phi/ (2*pi  { ) ) ) ; 
phi=phi— mult*2*pi  () ; 

end 

if  (phi  <  -2*pi  ( ) ) 

mult=floor{phi/ {-2*pi  {) ) ) ; 
phi=phi+mult*2*pi  () ; 

end 

if  (phi  <  0) 

phi=2*pi 0  +phi; 

end 

Ll_G_cent  (i, 9) =abs (phi) ; 

%Fix  and  reduce  angles 

theta=Ll_G_cent (i,  8)  ; 

theta=wrapTo2Pi (theta)  ; 
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if {theta>pi ( ) ) 

theta_a=theta-pi  ()  ; 
theta=pi ( ) -theta.a; 

end 

Ll_G_cent (i, 8) =theta; 


%SLMB 

%Angle  adjustment;  ensure  V  is  not  negative,  angles  in  proper  range 
phi=L2 _G_cent (i, 9) ; 
if  (phi  >  2*pi  { ) ) 

mult=f loor (phi/  (2*pi  { ) ) ) ; 
phi=phi— mult*2*pi  () ; 

end 

if  (phi  <  -2*pi  ( ) ) 

mult=floor{phi/ {-2*pi  () ) ) ; 
phi=phi+mult*2*pi  () ; 

end 

if  (phi  <  0) 

phi=2*pi 0  +phi; 

end 

L2_G_cent  (i, 9) =abs (phi) ; 

%Fix  and  reduce  angles 
theta=L2_G_cent (i,  8)  ; 
theta=wrapTo2Pi (theta)  ; 
if (theta>pi ( ) ) 

theta_a=theta-pi  ()  ; 
theta=pi ( ) -theta.a; 


end 
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L2_G_cent (i, 8) =theta; 


end 
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